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ABSTRACT 


This  thesis  addresses  modeling  and  simulation  of  the  human  lower  extremities  in 
order  to  track  walking  motion  and  estimate  walking  distance.  The  lower  extremities  are 
modeled  as  an  articulated  object,  which  consists  of  rigid  bars  connected  to  each  other  by 
joints. 

This  model  is  tested  by  using  both  synthetic  and  real  data.  The  synthetic  data  is 
created  based  on  the  main  principles  of  biomechanics.  The  real  data  is  obtained  from  the 
MARG  sensors  and  is  processed  by  the  Factored  Quaternion  algorithm.  Next,  it  is  im¬ 
plemented  in  a  simulation  program  written  in  Matlab.  The  program  utilizes  a  mathemati¬ 
cal  model  that  represents  the  human  gait-cycle  and  is  based  on  the  theory  of  forward 
kinematics  as  well  as  on  the  theory  of  manipulator  kinematics. 

The  simulation  program  is  able  to  track  the  motion  of  the  limbs  that  represent  the 
lower  extremities  and  estimate  the  traveled  distance.  Extensive  laboratory  tests  verified 
the  validity  of  the  configuration. 
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EXECUTIVE  SUMMARY 


This  thesis  aims  to  introduce  a  new  chapter  in  the  magnetic,  angular  rate,  and 
gravity  (MARG)  sensor  project.  The  objective  of  the  MARG  sensor  project  is  to  track 
human  motion,  based  on  inertial/magnetic  sensors,  which  measure  the  orientation  of  the 
human  limbs.  So  far  the  MARG  sensor  project  dealt  only  with  tracking  the  motion  of  the 
right  hand.  However,  in  order  to  implement  this  entire  project  to  build  motion-tracking 
body  suits  or  to  create  a  virtual  environment,  mainly  for  training  purposes  and  other  mili¬ 
tary  applications,  all  the  other  limbs  of  the  human  body  must  participate  as  well. 

The  main  objective  of  this  thesis  was  to  utilize  the  MARG  sensors  in  order  to 
track  the  motion  of  the  lower  extremities  and  estimate  the  position  of  the  model  that  exe¬ 
cutes  the  movement.  A  simulation  program  was  created  in  Matlab  in  order  to  accomplish 
this. 

For  the  purposes  of  this  research,  a  MARG  III  sensor  was  used.  The  sensor  is  at¬ 
tached  to  a  limb  and  data  is  obtained  as  the  limb  moves.  The  MARG  III  sensors  consist  of 
three  accelerometers,  three  magnetometers,  and  three  angular  rate  sensors  orthogonally 
mounted.  Thus,  data  is  derived  from  nine  sensor  elements  and,  in  order  to  be  wirelessly 
transmitted  to  a  central  terminal,  it  must  to  be  sent  through  a  single  channel.  Therefore,  it 
has  to  be  multiplexed.  This  is  the  task  of  a  control  interface  unit  (CIU).  This  thesis  uses  a 
three-channel  CIU. 

The  data  is  further  processed  by  the  Factored  Quaternion  algorithm,  which  trans¬ 
forms  the  “raw”  data  delivered  from  the  sensors  into  quaternions  and  then  into  angles. 
These  angles  represent  the  orientation  of  the  limb  in  all  directions.  Next,  the  sets  of  an¬ 
gles  are  provided  to  a  simulation  program,  which  implements  human  walking. 

Initially,  the  simulation  program  was  driven  by  pseudo-data,  which  represented 
small  increments  or  decrements  of  the  angles  of  the  limbs.  In  order  to  construct  this 
pseudo-data,  an  in-depth  research  on  the  fundamental  biomechanics  of  human  walking 
and,  especially,  of  human  gait-cycle  was  executed. 

xvii 


The  simulation  model  implemented  in  a  Matlab  program  is  based  on  the  theory  of 
forward  kinematics,  as  well  as  on  the  theory  of  manipulator  kinematics  due  to  the  sim¬ 
plicity  and  the  low  computational  load  they  ensure. 

The  results  of  the  simulation  using  real  data  were  satisfactory  since  the  program 
was  able  to  track  the  motion  of  the  lower  extremities  and  to  estimate  the  position  of  the 
model  with  desired  accuracy. 
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I.  INTRODUCTION 


This  chapter  discusses  the  previous  research  conducted  on  the  MARG  sensors 
and,  more  specifically,  on  their  implementation  in  tracking  human  motion.  Next,  the  the¬ 
sis  goals  are  presented  along  with  some  other  critical  issues  that  must  be  addressed.  Fi¬ 
nally,  a  brief  discussion  of  the  outline  of  the  thesis  chapters  follows. 

A.  MOTIVATION  AND  PREVIOUS  RESEARCH 

This  thesis  is  part  of  the  MARG  project.  The  MARG  project  is  an  on-going  effort 
to  track  human  motion  to  create  a  virtual  environment.  This  is  accomplished  by  utilizing 
magnetic,  angular  rate,  and  gravity  (MARG)  sensors,  which  are  inertial/magnetic  sensors. 
One  sensor  is  attached  to  one  limb  and  provides  data  according  to  the  motion  of  the  limb. 
Data  from  the  sensor  is  processed  via  a  control  interface  unit  (CIU)  and  an  algorithm  cal¬ 
culates  the  orientation  of  the  limbs.  As  it  can  be  easily  understood,  such  a  project  can  be 
applicable  to  virtual  combat  training  programs.  The  size  of  the  sensors,  which  is  consid¬ 
erably  small,  allows  them  to  be  attached  to  a  full  body  suit.  With  the  appropriate  auxiliary 
hardware  and  compatible  software,  tracking  the  motion  of  every  limb  is  possible. 

Eric  R.  Bachmann  [Ref.  1]  attempted  to  create  a  synthetic  environment  by  utiliz¬ 
ing  the  MARG  II  sensors.  One  sensor  was  attached  to  each  limb  and  data  from  all  sensors 
was  delivered  to  a  terminal,  where  it  was  processed  to  depict  the  motion  of  the  limbs  on  a 
human  avatar  (Figure  1).  However,  this  configuration  did  not  come  without  drawbacks. 
The  considerably  large  dimensions  and  the  weight  of  the  equipment  as  well  as  high 
power  consumption  were  problems  that  had  to  be  resolved.  Furthermore  the  data  trans¬ 
mission  was  not  wireless.  Andreas  Kavousanos-Kavousanakis  [Ref.  2]  presented  a  differ¬ 
ent  configuration,  in  which  data  transmission  was  wireless  and  the  dimensions  of  the 
equipment  along  with  the  power  consumption  were  significantly  reduced.  Furthermore, 
the  Quest  algorithm  was  implemented  to  calculate  the  orientation  of  the  limbs.  The  algo¬ 
rithm  used  the  properties  of  the  quaternions  and  was  able  to  track  the  motion  of  the  right 
ann  in  real  time,  by  utilizing  two  MARG  III  sensors  (one  at  each  limb).  However,  the 
Quest  algorithm  proved  to  be  inadequate  when  large  linear  acceleration  occurred.  In  or¬ 
der  to  address  that  problem,  Conrado  Aparicio  [Ref.  3]  designed  a  Kalman  filter  and  was 
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able  to  track  the  motion  of  the  right  arm  efficiently  in  real  time  even  when  large  linear 
accelerations  occurred.  The  MARG  III  sensors  were  also  utilized  in  this  case. 


Figure  1.  Initial  MARG  Project  Configuration  [After  Ref.  4.] 

This  thesis  attempts  to  move  the  MARG  project  to  another  level  by  studying  the 
motion  of  the  lower  extremities.  The  MARG  III  sensors  are  used  so  that  data  can  be  de¬ 
rived  and  transmitted  via  the  CIU  to  a  central  terminal.  Then,  the  data  is  processed  by  the 
Kalman  filter  and  imported  into  a  Matlab  program,  in  order  to  track  the  motion  of  the 
lower  extremities.  The  first  version  of  the  Matlab  program  consisted  of  pseudo-data, 
which  was  generated  based  on  the  basic  principles  of  biomechanics.  In  addition,  a 
mathematical  model,  based  on  the  theory  of  forward  kinematics  and  on  the  theory  of  ma¬ 
nipulator  kinematics,  was  implemented. 

B.  THESIS  GOALS 

The  main  goal  of  this  thesis  was  to  develop  a  Matlab  simulation  program  that 
models  the  lower  extremities  as  an  articulated  object,  tracks  their  motion,  and  estimates 
the  traveled  distance  of  the  model.  In  order  to  achieve  this  goal,  it  was  necessary  to  ad¬ 
dress  the  following: 
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•  Provide  a  description  of  walking  motion  and  analyze  the  human  gait- 
cycle, 

•  Derive  a  mathematical  model  to  describe  human  walking,  using  forward 
kinematics  and  manipulator  kinematics, 

•  Implement  the  mathematical  model  in  a  computer  simulation  in  order  to 
describe  the  full  gait-cycle, 

•  Use  the  MARG  III  sensors  to  provide  real  data, 

•  Process  the  data  and  implement  it  in  a  Matlab  program  in  order  to  track 
the  motion  of  the  lower  extremities  and  to  estimate  the  traveled  distance  of 
the  model. 

C.  THESIS  ORGANIZATION  OUTLINE 

Chapter  II  provides  a  theoretical  background  on  human  body  locomotion  and  dis¬ 
cusses  the  patterns  of  human  walking.  Furthermore,  it  analyzes  the  human  gait-cycle  and 
discusses  certain  attributes,  such  as,  the  location  of  motion,  the  direction  of  motion,  the 
determinants  of  gait,  and  other  crucial  parameters. 

Chapter  III  provides  a  discussion  of  kinematics  and  dynamics.  The  goal  of  this 
chapter  is  to  derive  a  mathematical  model  in  order  to  describe  human  walking  adequately. 
Forward  kinematics  theory  was  chosen  for  this  purpose  mostly  due  to  its  simplicity.  Fur¬ 
thermore,  in  order  to  model  the  lower  part  of  the  human  body,  a  discussion  in  manipula¬ 
tor  kinematics  is  provided. 

Chapter  IV  provides  a  computer  simulation  of  human  walking  by  implementing 
the  mathematical  model  presented  in  Chapter  III,  in  a  program  written  in  Matlab.  In  addi¬ 
tion,  this  chapter  discusses  several  past  approaches  of  simulating  human  walking. 

Chapter  V  presents  a  more  advanced  and  sophisticated  model,  which  represents 
the  lower  extremities,  and  is  implemented  with  a  program  written  in  Matlab  to  simulate 
the  human  gait-cycle  more  effectively.  In  addition,  a  program  in  LISP  is  illustrated  in 
order  to  describe  the  behavior  of  the  foot  during  the  human  gait-cycle.  Finally,  a  brief 
comparison  between  the  two  approaches  is  provided. 

Chapter  VI  discusses  the  concept  of  tracking  human  motion  and  the  concept  of 
position  estimation.  The  implementation  of  the  MARG  sensors  to  derive  real  data  is  dis¬ 
cussed.  Furthermore,  a  brief  description  of  the  equipment  is  provided.  This  chapter  also 
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discusses  the  processing  of  the  data  and  how  it  was  implemented  in  the  Matlab  simulation 
program  to  move  the  model  and  to  estimate  its  position.  Finally,  the  results  of  this  proce¬ 
dure  are  presented. 

The  final  chapter  presents  the  conclusions  derived  from  this  research  as  well  as 
some  suggestions  regarding  the  continuation  of  this  project  and  recommendations  for  fu¬ 
ture  work. 

Appendix  A  contains  a  Matlab  program  which  tests  the  mathematical  model  of 
the  human-gait  that  is  discussed  in  Chapter  III.  Appendix  B  contains  a  Matlab  simulation 
program  for  the  case  which  is  presented  in  Chapter  IV.  Appendix  C  contains  a  Matlab 
simulation  program  with  the  enhancements  presented  in  Chapter  V.  Appendix  D  contains 
a  LISP  program  written  by  Professor  Robert  McGhee  that  is  discussed  in  Chapter  V.  Ap¬ 
pendix  E  presents  a  Matlab  simulation  program  that  processes  real  data.  This  program  is 
discussed  in  Chapter  VI. 
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II.  DESCRIPTION  OF  MOTION-GAIT  ANALYSIS 


This  chapter  discusses  the  walking  model  of  the  human  body  and  provides  a  de¬ 
scription  of  walking  motion,  which  contains  a  brief  illustration  of  the  types  of  motion,  the 
location  of  motion,  and  the  direction  of  motion.  The  main  part  of  this  chapter  provides  a 
thorough  description  of  human  locomotion,  and  analyzes  the  gait-cycle  along  with  some 
other  crucial  parameters.  The  end  of  the  chapter  presents  another  group  of  components  of 
gait,  called  determinants  of  gait. 

A.  TYPES  OF  MOTION 

The  following  material  is  mainly  taken  from  [Ref.  5]. 

The  human  skeleton  could  be  described  as  a  system  which  contains  rigid  bars  and 
joints.  Rigid  bars  have  different  lengths  and  sizes,  depending  on  which  bone  they  repre¬ 
sent,  connecting  to  each  other  with  joints.  Describing  the  human  body  is  not  an  easy  task. 
One  leg  alone  has  29  bones  and  37  muscles  [Ref.  6].  However,  the  objective  of  this  thesis 
is  not  to  give  a  precise  description  of  the  biomechanics  function  of  the  lower  part  of  the 
body,  but  to  illustrate  a  model  used  in  future  chapters  to  derive  the  human  walking 
model. 

There  are  four  types  of  motion  that  describe  every  possible  move  of  which  an  ob¬ 
ject  is  capable.  The  angular  motion  is  the  motion  of  an  object  around  an  axis,  with  the 
object  having  a  constant  distance  from  the  axis  of  rotation.  Each  point  of  the  object 
moves  through  the  same  angle,  at  the  same  time.  As  seen  in  later  chapters,  although  joints 
present  a  rather  complex  movement,  their  movement  can  be  considered  purely  angular 
for  reasons  of  simplicity. 

The  second  type  of  motion  is  called  translatory  or  linear  motion.  In  this  kind  of 
movement,  the  object  travels  in  a  straight  line.  In  other  words,  all  parts  of  the  object 
travel  parallel  to  each  other  and  have  the  same  distance  at  the  same  time. 

The  combination  of  the  angular  and  translatory  motion  gives  the  third  type  of  mo¬ 
tion,  which  is  called  curvilinear  motion.  This  occurs  when  an  object  is  traveling  from  one 
point  to  another  while  at  the  same  time  it  is  rotating  around  an  axis.  A  classical  example 
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is  the  Earth,  which  is  traveling  into  space  while  it  is  rotating  around  its  own  axis.  How¬ 
ever,  this  is  more  complicated  since  the  Earth  rotates  around  the  sun,  and  our  solar  sys¬ 
tem  and  galaxy  travel  through  space  as  well. 

The  fourth  and  last  type  of  motion  is  called  general  plane  motion.  General  plane 
motion  is  very  similar  to  the  curvilinear  motion.  The  difference  is  that  the  object  is  seg¬ 
mented  and  free  to  move.  Walking,  as  well  as  most  of  the  movements  of  the  limbs  of  a 
human  being,  belongs  to  the  general  plane  motion  category.  For  example,  the  movement 
of  the  lower  limb  of  a  leg  contains  a  rotation  and  a  translation  and  has  no  fixed  way  of 
traveling.  The  rotation  of  the  limb  takes  place  around  the  vertical  axis,  which  goes 
through  the  joint  that  connects  the  lower  with  the  upper  limb  of  the  leg.  Translatory 
movement  of  the  limb  occurs  when  it  travels  (forwards  or  backwards,  upwards  or  down¬ 
wards)  from  one  position  to  another  during  the  gait-cycle.  This  kind  of  movement  is  not 
fixed,  although  there  are  certain  limitations  to  be  discussed  later.  On  the  other  hand,  there 
are  certain  parts  of  the  body  that  maintain  their  linear  identity  during  walking.  The  head 
is  a  characteristic  example  of  translatory  movement  during  walking. 

1.  Location  of  Motion 

A  proper  description  of  the  movement  of  an  object  should  include  the  place  or  the 
plane  where  the  movement  occurs.  Given  that  the  three-dimensional  coordinate  system  is 
used,  the  motion  of  the  object  could  take  place  in  the  horizontal,  frontal  or  sagittal  plane. 
For  clarification,  those  planes  with  respect  to  a  person  standing  in  an  anatomic  position 
are  defined1.  However,  it  is  first  necessary  to  define  the  coordinate  system.  The  positive 
Z-axis  has  the  forward  direction  of  the  standing  person,  the  positive  7-axis  goes  up  and 
the  positive  X-axis  is  found  according  to  the  right-hand  rule.  The  horizontal  plane  corre¬ 
sponds  to  the  X-Z  plane,  and  divides  the  body  into  lower  and  upper  halves  as  shown  in 
Figure  2a.  The  frontal  plane  corresponds  to  the  X-Y plane,  and  divides  the  body  into  front 
and  back  halves  as  shown  in  Figure  2b.  The  sagittal  plane  corresponds  to  the  Y-Z  plane, 
and  divides  the  body  into  right  and  left  halves  as  shown  in  Figure  2c.  When  an  object  is 
moving  in  one  of  these  planes,  it  is  either  rotating  around  its  axis  or  translating  from  one 
point  to  another,  with  a  path  parallel  to  the  plane  or  both.  Of  course,  it  is  not  possible  to 
restrict  human  movements  in  that  manner. 

1  An  anatomic  position  is  the  position  where  the  person  stands,  looking  forward. 
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Figure  2.  Florizontal,  Frontal  and  Sagittal  Plane  [From  Ref.  5.] 

2.  Direction  and  Quantity  of  Motion 

Descriptions  of  the  direction  of  movements  depend  on  the  nature  of  the  movement 
itself.  In  the  case  of  translatory  movement,  signs  are  given  conventionally.  Up  and  right 
motions  take  positive  values,  and  down  and  left  motions  take  negative  values. 

For  rotatory  motions,  signs  are  set  arbitrarily  to  clockwise  and  counterclockwise. 
Based  on  the  plane  on  which  the  motion  occurs,  the  following  categorization  applies: 

•  Flexion  and  extension  occur  in  the  sagittal  plane.  According  to  Norkin  and 
Levangie,  “Flexion  refers  to  rotation  of  one  or  both  bony  levers  around  a 
joint  axis  so  that  ventral  surfaces  are  being  approximated”  [Ref.  5].  When 
the  movement  occurs  in  the  same  plane  with  a  different  direction,  it  is 
called  extension. 

•  Abduction,  adduction,  and  lateral  flexion  mainly  occur  in  the  frontal 
plane.  According  to  Norkin  and  Levangie,  “Abduction  is  rotation  of  one  or 
both  segments  of  a  joint  around  an  axis  so  that  the  distal  segment  moves 
away  from  the  midline  of  the  body”  [Ref.  5].  The  movement  that  occurs  in 
the  same  plane  in  an  opposite  direction  is  called  adduction.  Lateral  flexion 
occurs  when  the  moving  segment  is  part  of  the  midline  of  the  body. 
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•  Medial  and  lateral  rotations  occur  in  the  horizontal  plane  of  the  body.  By 
quoting  Norkin  and  Levangie  again,  “Medial  rotation  refers  to  rotation 
towards  the  body’s  midline,  and  lateral  rotation  refers  to  the  opposite  mo¬ 
tion”  [Ref.  5]. 

Note  that  for  the  purposes  of  this  thesis,  movement  in  the  sagittal  plane,  and  there¬ 
fore  flexion  along  with  extension,  is  the  main  field  of  development  of  the  human  walking 
model.  Representing  the  human  full  gait-cycle  in  the  plane  involves  only  rotation  of  the 
limbs  in  a  sagittal  plane  around  a  frontal  axis. 

Rotatory  motion  is  measured  either  in  degrees  or  in  radians.  As  is  known,  360  de¬ 
grees  correspond  to  2n  radians  (approximately  6.28  radians),  and  one  radian  corresponds 
to  57.3  degrees.  In  translatory  motion,  the  distance  that  the  object  travels  (displacement) 
is  being  measured.  Units  of  meters  will  be  used. 

B.  GAIT-CYCLE 

The  following  material  is  mainly  taken  from  [Ref.  7]. 

Human  gait  has  been  a  field  of  interest  and  research  since  prehistoric  times.  From 
the  cave  drawings  of  Cro-Magnon  to  the  sculptures  and  the  athletic  activities  depicted  on 
the  pottery  art  works  in  ancient  Greece  (Figure  3),  people  seemed  to  have  a  particular  in¬ 
terest  in  the  patterns  of  movement  of  humans  and  animals.  Of  course,  from  a  scientific 
point  of  view,  gait  science  has  been  rapidly  developing  during  the  past  decades.  Due  to 
recent  progress,  scientists  currently  are  capable  of  recording  and  analyzing  all  kinds  of 
movements  from  the  gait-cycle  of  a  child  to  the  perfonnance  of  an  athlete  [Ref.  8]. 

The  main  characteristic  of  human  gait  is  that,  just  like  voice  or  fingerprints,  it  is 
almost  unique  for  every  person.  One  may  claim  that,  in  general  terms,  the  gait-cycle  of  a 
person  is  similar  to  the  gait-cycle  of  every  other  individual,  which  is  exactly  the  point.  It 
may  be  similar  but  it  is  not  exactly  the  same.  Every  person  has  a  unique  characteristic 
gait  pattern.  Gait  pattern  varies,  depending  on  the  health  status,  personality,  occupation, 
age,  sex  and  many  other  attributes.  Some  characteristic  gait  patterns  are  those  of  a  soccer 
player,  a  military  person,  or  a  sailor.  Apart  from  the  individualistic  nature  of  the  human 
gait,  each  of  us  adjusts  the  gait  to  the  various  circumstances  of  every  day  life.  For  exam¬ 
ple,  sometimes  a  defensive  gait  pattern  is  being  developed.  Other  times,  it  is  an  offensive 
one  or  a  gait  pattern  developed  when  someone  is  in  a  hurry. 
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Figure  3.  Prehistoric  and  Ancient  Depictions  of  Fluman  and  Animal  Locomotion 

[After  Refs.  9  and  10.] 

The  reason  for  the  different  kinds  of  gait  patterns  is  that  gait  depends  on  certain 
attributes  such  as  the  construction  of  the  bones,  the  position  and  function  of  neuromuscu¬ 
lar  structures,  and  the  function  of  the  joints.  However,  the  detailed  explanation  of  such 
operation  is  beyond  the  scope  of  this  paper.  The  objective  is  to  illustrate  how  human  lo¬ 
comotion  is  achieved  in  terms  of  the  gait-cycle. 

When  someone  is  walking,  the  overall  movement  observed  from  the  outside  is 
purely  a  translatory  movement.  This  translation  of  the  body  takes  place  through  rotatory 
movements  of  the  lower  limbs,  which  in  a  healthy  person,  are  in  complete  harmony  with 
each  other.  Therefore,  the  lower  extremities  or,  in  other  words,  the  feet,  the  legs,  and  the 
thighs  are  cooperating  with  each  other  in  order  to  move  the  HAT  (head,  arms  and  trunk). 
The  term  gait-cycle  refers  to  the  period  of  time  between  two  identical  positions  of  the 
same  lower  extremity. 

1.  Phases  of  the  Gait-Cycle 

In  order  to  be  able  to  understand  gait  better,  scientists  divided  human  walking  into 
many  parts.  One  of  the  pioneers  was  an  American  prosthesist,  A.  A.  Mark,  who  more 
than  a  century  ago,  presented  medical  society  with  an  analysis,  in  which  he  divided  the 

gait  in  eight  contiguous  phases.  He  also  implemented  kinetoscopic  photography  in  order 
to  present  a  qualitative  analysis  (Figure  4).  In  1885,  a  French  physiologist  named  A. 

Marey  used  a  method  similar  to  Mark’s  to  depict  displacements  in  human  gait. 
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Cal  A  8. 


Figure  4.  The  Eight  Contiguous  Phases  of  the  Gait  According  to  A.  A.  Mark 

[From  Ref.  7.] 


Since  then,  thousands  of  papers  have  been  written  and  countless  presentations 
given,  trying  to  provide  more  insight  into  this  subject.  Different  perspectives  gave  differ¬ 
ent  interpretations  on  the  manner  in  which  the  gait  should  be  divided.  Depending  on 
whether  the  author  was  an  orthopedic,  a  physician  or  a  mathematician,  gait  was  defined 
in  various  ways.  Flowever,  for  the  sake  of  consistency,  only  one  description  of  gait-cycle 
is  presented  in  this  thesis.  According  to  the  professional  staff  at  Rancho  Los  Amigos 
Medical  Center,  in  California,  walking  involves  three  main  tasks  [Ref.  11]: 

•  Weight  acceptance 

•  Single-limb  support 

•  Swing  limb  advancement 

The  gait-cycle  starts  at  the  point  of  initial  contact  and  ends  when  the  very  same 
contact  occurs  again.  During  the  gait-cycle,  extremities  pass  through  two  phases,  a  single 
stance  phase  and  a  single  swing  phase  [Ref.  5].  The  stance  phase  occurs  for  as  long  as  the 
reference  lower  extremity  is  in  contact  with  the  ground.  It  starts  at  the  initial  contact  of 
the  extremity,  which  is  when  the  heel  touches  the  ground,  and  it  is  completed  when  the 
toe  leaves  the  ground.  The  stance  phase  comprises  62  percent  of  the  gait-cycle. 
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The  swing  phase  refers  to  the  part  of  the  cycle  in  which  the  same  extremity  does 
not  touch  the  ground.  It  starts  right  after  the  moment  the  toe  leaves  the  ground  and  it  is 
completed  right  before  the  moment  of  impact  between  the  heel  and  the  ground.  The 
swing  phase  encompasses  approximately  38  percent  of  the  gait-cycle. 

Another  important  period  of  human  locomotion  is  the  double  support,  which  oc¬ 
curs  when  both  lower  extremities  are  in  contact  with  the  ground  at  the  same  time.  There 
are  two  double  support  periods  in  one  cycle.  Those  two  periods  of  double-limb  support 
represent  25  percent  of  the  gait-cycle.  Figure  5  illustrates  the  phases  of  the  gait-cycle. 


Figure  5.  Phases  and  Subdivisions  of  Gait-cycle  [From  Ref.  12.] 


2.  Subdivisions 

Stance  is  further  divided  into  five  subunits:  heel  strike,  foot  flat,  midstance,  heel 
off,  and  toe  off.  Heel  strike  refers  to  the  moment  that  the  heel  strikes  the  ground.  Foot  flat 
refers  to  the  period  that  the  foot  is  completely  attached  to  the  ground.  Midstance  is  the 
moment  where  the  HAT  is  approximately  aligned  to  the  supporting  extremity.  Heel  off  is 
the  moment  where  the  heel  loses  contact  with  the  ground.  Finally,  toe  off  refers  to  the 
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moment  where  the  toe  is  in  contact  with  the  ground.  Note  that  a  reference  extremity  has 
been  defined  (either  right  or  left). 

Swing  is  divided  into  three  subunits:  acceleration,  midswing,  and  deceleration. 
Acceleration  begins  immediately  after  toe  off  of  the  reference  and  is  completed  when  the 
HAT  is  aligned  to  the  other  extremity.  Midswing  occurs  when  the  reference  extremity 
passes  beneath  the  HAT.  Deceleration  begins  right  after  midswing.  During  this  period, 
the  knee  is  extending  and  is  ready  for  heel  strike.  It  ends  right  before  the  heel  strike, 
which  initiates  a  new  gait-cycle. 

The  professional  staff  at  the  Rancho  Los  Amigos  (RLA)  Medical  Center  has  re¬ 
cently  presented  another  set  describing  the  subunits  has  been  presented  recently,  the  RLA 
method.  This  method  describes  the  gait  and,  unlike  the  traditional  tenninology,  it  refers  to 
lengths  of  time,  giving  a  more  adequate  definition  of  the  starting  and  ending  points  of  a 
subunit.  Table  1  compares  traditional  and  RLA  terminology.  In  addition,  Figure  6  illus¬ 
trates  the  phases  of  the  gait-cycle  along  with  its  subdivisions  in  both  traditional  and  RLA 
tenninology. 


Traditional 

RLA 

Heel  Strike 

Initial  Contact 

Heel  Strike  to  Foot  Flat 

Loading  Response 

Foot  Flat  to  Midstance 

Midstance 

Midstance  to  Heel  off 

Terminal  Stance 

Toe  off 

Preswing 

Toe  off  to  Acceleration 

Initial  Swing 

Acceleration  to  Midswing 

Midswing 

Midswing  to  Deceleration 

Terminal  Swing 

Table  1.  Comparison  between  Traditional  and  RLA  [After  Ref.  5.] 
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Figure  6.  Gait  Phase  Diagram  [From  Ref.  12.] 

3.  Time  and  Distance  Parameters  of  Motion 

The  following  material  is  mainly  taken  from  [Ref.  5]. 

This  section  discusses  several  other  characteristics  of  human  locomotion. 

The  time-related  characteristics  of  the  gait  are  called  temporal  variables.  Such 
variables  are:  stance  time,  single-limb  time,  double-support  time,  swing  time,  stride  and 
step  time,  cadence,  and  speed.  Stance  time  is  the  period  of  time  in  which  the  stance  phase 
occurs.  Single-limb  time  is  the  period  of  time  in  which  HAT  is  supported  by  one  extrem¬ 
ity.  Double-support  time  is  the  period  of  time  in  which  both  lower  extremities  are  in  con¬ 
tact  with  the  ground.  Stride  time  is  synonymous  to  gait-cycle  duration.  Step  time  is  the 
total  amount  of  time  required  for  one  step.  Cadence  is  the  number  of  steps  per  unit  of 
time  (steps  per  second  or  steps  per  minute).  Speed  is  referred  to  as  free,  slow  and  fast. 
Free  speed  refers  to  a  person’s  normal  and  undistracted  walking  speed,  while  slow  and 
fast  speeds  are  rather  self-explanatory  terms. 

The  distance  related  parameters  of  motion  are  called  distance  variables  and  in¬ 
clude  stride  length,  step  length,  width  of  walking  base,  and  degree  of  toe  out.  Stride 
length  is  the  linear  distance  from  one  heel  strike  to  the  next  for  the  same  lower  extremity. 
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Stride  length  increases  as  the  speed  increases  [Ref.  13].  Step  length  is  the  linear  distance 
from  one  heel  strike  of  one  extremity  to  the  next  heel  strike  of  the  opposite  extremity. 

The  width  of  the  base  support  is  the  linear  distance  between  one  point  of  the  heel  of  one 
lower  extremity,  and  the  same  point  of  the  heel  of  the  opposite  extremity.  Degree  of  toe 
out  is  the  angle  of  foot  placement. 

C.  DETERMINANTS  OF  GAIT 

The  following  material  is  mainly  taken  from  [Ref.  7]. 

Another  group  of  components  being  considered  quite  often  in  kinematics  analysis 
is  called  detenninants  of  gait.  During  walking,  the  center  of  gravity  (COG)  of  the  body 
makes  a  translatory  movement.  The  determinants  of  gait  are  trying  to  maintain  the 
movement  of  the  COG  to  a  minimum.  There  are  six  determinants:  variations  in  pelvic 
rotation,  pelvic  tilt,  knee  flexion  at  midstance,  foot  and  ankle  motion,  knee  motion,  and 
lateral  pelvic  displacement. 

There  are  forward  and  backward  pelvic  rotations  which  directly  relate  to  the  for¬ 
ward  and  backward  movements  of  the  lower  extremities.  The  total  range  of  pelvic  rota¬ 
tion  is  eight  degrees,  four  for  the  forward  and  four  for  the  backward  rotation.  Figure  7 
illustrates  the  pelvic  rotation  in  the  transverse  plane. 


Figure  7.  Pelvic  Rotation  [From  Ref.  7.] 
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The  COG  reaches  its  highest  point  at  midstance.  Note  that  the  trajectory  of  the 
COG  in  walking  has  a  sinusoidal  fonn.  This  highest  point  would  be  even  higher  if  it  was 
not  for  the  pelvic  tilt  to  depress  the  COG.  The  total  range  of  the  tilt  towards  the  swing 
side  is  five  degrees  from  vertical.  Figure  8  demonstrates  the  pelvic  tilt  in  the  frontal 
plane. 


Figure  8.  Pelvic  Tilt  [From  Ref.  7.] 


Knee  flexion  is  another  adjustment  of  the  body  that  prevents  the  COG  from  going  even 
higher. 

The  foot  and  ankle  motion  are  perhaps  the  most  important  mechanisms  in  smooth¬ 
ing  the  trajectory  of  the  COG  during  the  stance  phase.  They  contribute  to  that  task  by 
keeping  the  trajectory  of  the  COG  in  a  horizontal  position  as  depicted  in  Figure  9. 


Figure  9.  Foot  and  Ankle  Motion  [From  Ref.  7.] 


Knee  motion  is  directly  related  to  foot  and  ankle  motion,  and  it  also  helps  in 
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smoothing  the  trajectory  of  the  COG.  In  general  terms,  it  could  be  claimed  that,  when  the 
ankle  is  depressed,  the  knee  extends.  Similarly,  when  the  ankle  is  elevated,  the  knee 
flexes  [Ref.  7],  as  seen  in  Figure  10. 


Figure  10.  Knee  Motion  in  Synchronization  with  Foot  and  Ankle  Motion 

[From  Ref.  7.] 

Finally,  lateral  pelvic  displacement  is  the  factor  that  improves  the  position  of  the 
COG  over  the  support  limb  as  depicted  in  Figure  1 1 . 


Figure  11.  Lateral  Pelvic  Displacement  [From  Ref.  7.] 


D.  SUMMARY 

This  chapter  discussed  the  basic  elements  of  human  walking  and  provided  an  in¬ 
sight  into  human  locomotion.  The  gait-cycle  along  with  its  subdivisions  was  introduced, 
and  additional  infonnation  concerning  the  time  and  distance  parameters  of  walking  gait 
presented.  Finally,  the  last  part  of  the  chapter  discusses  the  determinants  of  gait.  The  fol¬ 
lowing  chapter  discusses  forward  kinematics  and,  in  particular,  it  introduces  manipulator 
kinematics.  Thus,  the  knowledge  obtained  from  this  chapter  will  be  used  in  the  next  chap¬ 
ter  to  derive  a  mathematical  expression  of  a  human  walking  model  representation. 
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III.  MATHEMATICAL  MODEL 


This  chapter  provides  a  brief  discussion  of  kinematics  and  dynamics.  It  focuses  on 
the  study  of  forward  kinematics,  which  is  used  for  the  derivation  of  the  mathematical 
model  of  the  human  gait.  The  reason  for  this  is  that  the  kinematic  approach  is  simpler 
than  the  dynamic  one,  and  it  is  more  capable  of  providing  a  sufficient  representation  of 
the  locomotion  of  the  lower  extremities  of  a  human  being.  The  biomechanics  fundamen¬ 
tals  presented  in  the  previous  chapter  are  an  essential  factor  in  implementing  forward 
kinematics  in  the  human  walking  model  illustrated  in  this  chapter. 

The  lower  part  of  the  human  body  is  modeled  as  an  articulated  object  constructed 
of  rigid  parts  (links)  connected  with  each  other  by  joints.  In  order  to  estimate  the  position 
and  the  orientation  of  the  rigid  parts  of  the  model,  a  discussion  in  manipulator  kinematics 
is  found  to  be  helpful. 

After  deriving  the  mathematical  model  which  computes  the  position  and  orienta¬ 
tion  of  the  links  representing  the  lower  extremities,  a  Matlab  program  was  written  to  test 
the  validity  of  the  model  based  on  the  numerical  solutions.  This  model  was  used  in  the 
next  chapter  to  simulate  human  walking  based  on  pseudo-data. 

A.  BACKGROUND 

There  are  two  ways  to  create  physically-based  mathematical  models  for  human 
motion  simulation,  kinematics  and  dynamics.  Kinematic  models  are  very  simple  to  im¬ 
plement,  and  do  not  have  as  large  a  computational  load  as  dynamics.  Their  drawback  is 
that  they  depend  on  the  geometry  of  human  body  parts  [Ref.  14].  In  dynamic  models,  all 
the  forces  and  torques  which  contribute  to  the  development  of  the  gait  are  calculated.  On 
the  contrary,  in  kinematic  models,  the  forces  and  torques  are  not  involved.  However,  dis¬ 
placements,  velocities,  and  accelerations  are  studied.  Since  one  of  the  objectives  of  this 
thesis  was  the  study  of  human  walking,  it  is  natural  to  focus  on  the  kinetics  and,  espe¬ 
cially,  on  the  kinematics  of  lower  limbs  (extremities)  [Ref.  15]. 

According  to  Graig,  “Kinematics  is  the  science  of  motion  which  treat  motion 
without  regard  to  the  forces  which  cause  it”  [Ref.  16].  Since  the  study  of  kinematics  con¬ 
cerns  position  and  all  its  derivatives,  such  as  velocity  and  acceleration,  kinematics  of  ma- 
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nipulators  refers  to  the  geometry  and  the  time -based  properties  of  the  motion.  The  human 
body  could  be  thought  of  as  a  mechanical  manipulator  consisting  of  many  rigid  parts 
connected  with  joints.  Hence,  the  lower  parts  of  the  body  could  be  thought  of  as  a  five- 
rigid-part  mechanical  manipulator  (including  a  link  to  describe  the  pelvis).  For  this  me¬ 
chanical  manipulator,  a  mathematical  model  of  walking  must  be  derived  based  on  the 
theory  of  forward  kinematics. 

In  forward  kinematics,  given  a  set  of  angles,  the  position  and  orientation  of  each 
part  of  the  mechanical  manipulator  can  be  calculated.  The  calculation  of  the  position  and 
orientation  of  the  last  part  of  the  chain,  which  is  called  the  end-effector,  is  important.  On 
the  other  hand,  inverse  kinematics  presents  the  exact  opposite  problem.  Given  the  posi¬ 
tion  and  orientation  of  the  end-effector,  the  joint  angles  are  calculated.  Generally,  the  po¬ 
sition  of  the  manipulator  is  described  by  comparing  the  axis-frame,  which  is  attached  to 
the  end-effector  to  the  axis-frame  that  is  attached  to  the  base  of  the  manipulator  [Ref.  16] 
(Figure  12). 


Figure  12.  Tool  Frame  Related  to  the  Base  Frame  [From  Ref.  16.] 

Dynamics  involves  the  study  of  forces  and  torques  in  generating  the  human  gait. 
When  the  forces  and  torques  are  to  be  detennined,  given  the  behavior  of  each  link,  the 

problem  is  called  the  inverse  dynamics  problem.  On  the  other  hand,  when  the  torques  are 
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given  and  the  accelerations  are  to  be  computed,  the  problem  is  called  the  direct  dynamics 
problem.  Problems  such  as  representing  the  movements  of  certain  body  parts  due  to  the 
function  of  the  muscles  require  a  dynamics  approach  such  as  Newton-Euler  mechanics 
[Ref.  17]. 

B.  MANIPULATOR  KINEMATICS 

The  materials  presented  in  this  section  are  mainly  drawn  from  [Ref.  16]. 

As  previously  stated,  kinematics  deal  with  the  study  of  motion  without  involving 
the  forces  and  the  torques  applied  to  the  body.  It  involves  the  calculation  of  position  as 
well  as  the  derivatives  of  position  (velocity  and  acceleration).  Kinematics  of  manipulators 
refers  to  the  geometry  of  the  body  and  all  of  its  time-based  parameters.  The  way  to  han¬ 
dle  manipulator  kinematics  is  to  define  frames  on  every  link  and  then  to  study  how  these 
frames  connect  to  each  other.  Another  point  of  study  is  to  see  how  these  frames  change  as 
the  body  articulates. 

Manipulators  consist  of  rigid  links  connected  with  joints.  There  are  six  different 
types  of  joints.  However,  only  two  of  them  are  used  on  a  grand  scale,  since  they  exhibit 
one  degree  of  freedom,  and  manipulators  are  generally  constructed  with  joints  that  have 
only  one  degree  of  freedom.  Therefore,  revolute  and  prismatic  joints  are  the  most  com¬ 
mon  (Figure  13).  When  a  joint  of  a  mechanism  exhibits  n  degrees  of  freedom,  it  is  ana¬ 
lyzed  into  n  - 1  links  of  zero  length  connected  with  joints  that  exhibit  only  one  degree  of 
freedom.  A  large  number  of  characteristics  of  the  link  exist,  including  the  type  of  mate¬ 
rial,  strength  and  shape  of  the  link,  and  location  and  type  of  the  joints.  Nevertheless,  for 
the  purposes  of  this  thesis,  a  link  is  considered  only  as  a  rigid  body  which  defines  the  re¬ 
lationship  between  two  neighboring  joints. 
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Figure  13.  The  Six  Common  Types  of  Joints  [From  Ref.  18.] 

1.  Link  Parameters 

Before  analyzing  the  link  parameters,  another  definition  concerning  the  joint  axis 
is  in  order.  The  joint  axis  is  a  line  in  space  about  which  link  n  rotates  with  respect  to  link 
n  - 1 . 

In  a  manipulator,  each  link  has  four  link  parameters:  link  length,  link  twist,  link 
offset  and  joint  angle.  The  length  of  a  line  that  is  mutually  perpendicular  to  both  joint 
axes  is  called  link  length.  Link  length  a;  l  involves  joint  axes  i  and  /  - 1 . 

The  second  parameter  is  called  link  twist.  The  link  length  and  link  twist  define  the 
relative  location  of  the  two  axes.  The  a  M  angle,  measured  from  axis  i- 1  to  axis  i ,  us¬ 
ing  the  right-hand  rule,  is  called  link  twist. 

The  following  two  parameters  are  used  to  describe  how  two  neighbor  links  are 
connected  to  each  other.  Neighbor  links  have  a  common  joint  axis  in  the  middle.  The  first 
parameter  refers  to  the  distance  measured  along  the  common  axis  between  two  neighbor 
links  (axis  / ),  from  the  point  where  a.  ,  intersects  axis  i  to  the  point  where  ai  intersects 
axis  / ,  and  is  called  the  link  offset.  The  link  offset  at  link  axis  i  is  dt . 
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The  second  parameter  refers  to  the  relative  angle  between  one  link  and  its 
neighbor  and  is  called  the  joint  angle  6i .  Out  of  four  link  variables,  the  first  two  describe 

the  link  itself,  and  the  other  two  describe  how  this  link  is  connected  to  its  neighbor  links. 
Figure  14  illustrates  the  link  parameters  of  one  link  of  a  manipulator. 


Figure  14.  Link  Parameters  [From  Ref.  18.] 

2.  Convention  in  Locating  Frames 

One  frame  is  attached  to  each  link.  In  order  to  describe  the  relations  between 
neighboring  links,  the  following  convention  is  followed. 

Assume  that  the  /  -th  frame  is  attached  to  the  /  -th  link.  It  is  a  three-dimensional 
frame  where  the  Z-axis  is  the  same  as  the  joint  axis  / .  The  X-axis  is  pointing  towards  the 
direction  of  a, ,  where  a,  is  the  length  of  link  / .  The  direction  of  the  7-axis  is  detennined 
based  on  the  right-hand  rule.  Note  that  the  origin  of  the  frame  coincides  with  the  point 
where  a,  intersects  the  joint  /-axis. 

An  alternative  definition  of  the  link  parameters  according  to  the  frame  convention 
is  given  below. 

•  ai  refers  to  the  distance  from  Z(  to  Zi+I  measured  along  X. . 

•  aj  refers  to  the  angle  between  Z(  and  Zi+1  measured  along  X. . 
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•  cl  refers  to  the  distance  from  Xi  I  to  Xt  measured  along  Z( . 

•  Qi  refers  to  the  angle  between  X [  j  and  X.  measured  along  Z( . 

Figure  15  illustrates  a  three-link  manipulator  with  all  its  joints  being  revolute. 

There  are  three  frames  attached,  one  for  each  limb,  plus  the  reference  zero-frame  for  a 
total  of  four  frames.  In  addition,  every  link  length  and  joint  angle  is  depicted  along  with 
the  corresponding  link  parameters. 


i 

<*i  -  1 

a,  -  l 

dt 

0, 

i 

0 

0 

0 

«1 
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Li 
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02 
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0 

Li 

0 

Os 

Figure  15.  Three-Link  Manipulator  [From  Ref.  18.] 

3.  Mappings  Involving  Translated  and  Rotated  Frames 

Before  presenting  the  mathematical  model  of  the  manipulator,  which  represents 
the  orientation  and  position  of  the  lower  extremities,  a  small  digression  is  in  order.  It  is 
important  first  to  gain  insight  into  the  mappings  involving  translated  and  rotated  frames, 
and  second  to  understand  the  meaning  and  the  utility  of  transformation  matrices. 

When  pure  translation  occurs,  a  point  P  in  space,  relative  to  an  initial  frame  {A}, 
is  described  as  follows2. 

aP=bP+aPborg  (3-1) 


2  Notation  is  such  that  vectors  have  leading  superscripts  and  matrices  have  leading  superscripts  and 
subscripts. 
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where  AP  is  the  vector  that  locates  point  P  relative  to  frame  {A},  BP  is  the  vector  that 
locates  point  P  relative  to  frame  {B},  and  APBORG  is  the  vector  that  locates  the  origin  of 
frame  {B}  relative  to  frame  {A}. 

When  a  pure  rotation  occurs,  a  point  P  in  space,  relative  to  an  initial  frame  {A}, 
is  described  as  follows. 

AP=ARBP  (3-2) 

where  BR  is  the  rotation  matrix  that  describes  frame  {B}  relative  to  frame  {A}.  How¬ 
ever,  there  are  cases  in  which  both  translation  and  rotation  occur  (Figure  16).  Therefore, 
the  mapping  of  a  vector  from  its  description  in  one  frame  to  a  description  to  another 
frame  is  given  as. 

aP=abRbP+aPborg  (3-3) 

or 

aP=abTbP  (3-4) 

where 

Arrr  _ 

B1  ~ 

0 


R 


0  0 


1  BORG 


(3-5) 
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AP  =  abRbP  +  aPBOrg 


Figure  16.  General  Transformation  [After  Ref.  19.] 


This  matrix  includes  the  rotation  and  translation  of  the  general  case.  Thus,  trans¬ 
formation  matrices  are  used  to  specify  a  frame.  In  the  case  of  manipulator  kinematics,  the 
transformation  matrix,  which  relates  two  frames  attached  to  neighbor  links,  is  given  by 


cos6>  -sin  6]  0 

sin 0t cos cos6*cos ai_]  -sinoy, 
sin^.sinaM  cost?  sin  or,, ,  cosa,._, 
0  0  0 


-  d:  sino;i_1 
dj  coscrM 

1 


(3-6) 


Therefore,  the  first  step  is  to  define  the  frames  attached  to  each  link.  Next,  the  corre¬ 
sponding  link  parameters  to  each  frame  must  be  calculated.  Then,  the  transformation  ma¬ 
trices  corresponding  to  each  set  of  link  parameters  can  be  derived.  Finally,  by  multiplying 
the  individual  transfonnation  matrices  from  \T  to N  '/T  ,  the  general  transformation  ma¬ 
trix  “r  can  be  obtained  as 


lT=\T$\T . N;2JN~'NT.  (3-7) 

C.  MATHEMATICAL  MODEL  FOR  HUMAN  LOWER  EXTREMITIES 

Figure  17  illustrates  a  manipulator  model  which  describes  the  lower  extremities  of 
the  human  body  and  consists  of  five  links.  Each  leg  consists  of  two  links,  one  thigh  and 
one  shank.  One  link  is  assigned  to  represent  the  pelvis.  The  foot  is  not  included  in  this 
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model  due  to  the  complexity  and  the  significant  computational  load  it  would  cause.  Fur¬ 
thermore,  after  deriving  the  mathematical  model,  a  computer  simulation  of  human  walk¬ 
ing  is  illustrated,  using  pseudo-data,  in  order  to  calculate  the  traveled  distance.  Next,  real 
data  is  used  by  utilizing  the  MARG  sensors. 


Right  leg 


Figure  17.  Manipulator  Describing  Lower  Extremities 

Figure  17  also  shows  a  two-dimensional  scheme  in  which  the  lower  extremities 
are  in  the  midstance,  right  before  the  double-support  phase.  Link  one  represents  the  right 
shank,  link  two  represents  the  right  thigh,  link  three  represents  the  pelvis,  link  four  repre¬ 
sents  the  left  thigh  and  finally,  link  five  represents  the  left  shank.  The  links  are  connected 
to  each  other  with  revolute  joints.  Link  one  is  connected  with  a  revolute  joint  to  the  base 
of  the  manipulator  (ground). 

As  is  shown,  the  frames  are  attached  to  the  links.  Also  the  JAaxes  coincide  with 
the  links,  and  the  7-axes  form  a  90-degree  angle  with  the  A-axcs  and  Z-axes  being  ver¬ 
tical  to  the  plane  indicating  that  all  link  twists  are  equal  to  zero. 

Table  2  presents  the  link  parameters  of  each  frame,  where  /  ;  the  length  of  the 
pelvis  and  (p  is  the  angular  displacement  of  the  pelvis  in  the  7-axis. 
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ai-i 

ai-l 

d, 

0, 

Link  1 

0 

0 

0 

Link  2 

0 

k 

0 

#2 

Link  3 

0 

4 

0 

Link  4 

0 

lpelS[n(P 

0 

Link  5 

0 

h 

0 

Table  2.  Link  Parameters 


Next,  the  individual  transformation  matrices  for  each  pair  of  neighboring  links  are 
defined.  For  frames  zero  and  one,  the  transformation  matrix  \T  is  as  follows: 

cos#,  -sin#,  0 

0  sin#,  cos#,  0 

,T=  0  0  1 

0  0  0 

and,  for  frames  one  to  two, 

cos#2  -sin#2  0 

,  sin#2  cos#2  0 

lT=  0  "  0  1 

0  0  0 

Before  defining  the  transformation  matrix  \T ,  a  brief  discussion  concerning  the 
motion  of  the  pelvis  is  in  order.  Let  lpel  be  the  length  of  the  pelvis.  During  a  step  forward, 

the  maximum  angular  displacement  in  the  X-axis  is  approximately  three  degrees.  The 
corresponding  angular  displacement  in  the  7-axis  is  minus  three  degrees.  As  previously 


0 

0 

1 


(3-9) 


0 

0 

0 

1 


(3-8) 


26 


stated,  this  is  a  two-dimensional  approach.  Therefore,  frame  three  can  be  defined  as 
shown  in  Figure  17,  without  any  loss  of  generality.  From  the  values  of  0X ,  02  and  cp ,  the 

angle  02  is  calculated  by  using: 


6*3  =  360  -02  -6x+(p . 


Hence,  the  transformation  matrix  \T  is  equal  to: 


Similarly, 


and 


cos  6*3 
sin  6*3 
0 
0 


cos  6*4 
sin  6*4 
0 
0 


cos  6*5 
sin  6*5 
0 
0 


-sin  6*3 
cos  6*3 
0 
0 


-sin  6*4 
cos  6*4 
0 
0 


-sin  6*5 
cos  6*5 
0 
0 


0 

0 

1 

0 


0 

0 

1 

0 


0 

0 

1 

0 


h 

0 

0 

1 


lpelsin(P 

0 

0 

1 


h 

0 

0 

1 


(3-10) 


(3-11) 


(3-12) 


(3-13) 


Finally,  Equation  (3-8)  gives  the  transformation  matrix  that  relates  frame  five  to 
frame  zero: 


cos(#j  +  ...  +  05) 


sin^  +  ...  +  05) 

0 

0 


-sin(#j  +  ...  +  05) 

cos(#j  +  ...  +  05) 

0 

0 


0  /3  cos(6(  + . . .  +  6*4 )  +  lpel  smcp  cos(6*  +  ...  +  03) 
+ l2  cos(0x  +  02)  +  lx  COS0X 
0  /3  sin(^  + . . .  +  6*4 )  +  lpel  sin#?  sin(^  +  ...  +  03) 
+ 12  sin(0j  +02)  +  Ix  sin^ 

1  0 

0  1 


(3-14) 
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The  coordinates  of  the  end-effector  with  respect  to  frame  five  are: 


5P  =  [/4  0  0  If. 


(3-15) 


Hence,  by  using  Equation  (3-5),  the  coordinates  of  the  end-effector  with  respect 
to  the  reference  frame  zero  are: 


'P  = 


l4  cos(6*1  +  ...  +  #5)  +  /3cos(#1  +  ...  +  6l4)  +  //;Wsin^cos(6,1  +  ...  +  03) 
+ l2  cos(6*1  +  02)  +  l4  cos^ 

l4  sin^  +  ...  +  6,5)  +  /3sin(6,]  + ...  +  64)  +  lpelsm<psm(6x  +  ...  +  #,) 
+ 12  sin(#j  +  02)  +  lx  sin^ 


0 


.  (3-16) 


1.  Testing  the  Model 

In  order  to  verify  that  the  mathematical  model  presented  above  is  correct,  a  pro¬ 
gram  was  written  in  Matlab.  This  code  evaluated  the  model  by  checking  the  results  of  the 
numerical  solutions.  The  code  is  presented  in  Appendix  A. 


Table  3  illustrates  the  fixed  link  parameters  used  in  the  code. 


a._j  (degrees) 

a  t_j  (meters) 

d{  (meters) 

(degrees) 

Link  1 

0 

0 

0 

60 

Link  2 

0 

0.45 

0 

30 

Link  3 

0 

0.4 

0 

270 

Link  4 

0 

0.2  sin  — 

60 

0 

300 

Link  5 

0 

0.4 

0 

345 

Table  3.  Fixed  Link  Parameters 


28 


After  running  the  program,  the  following  results  are  obtained. 


"0.2250” 

”0.2355” 

”0.4355” 

”0.5519” 

0.7897 

,  °p  = 

0.7792 

°p  = 

*>  A 

0.4433 

,  and  °P  = 

0.0086 

0.0000 

7  J 

0.0000 

7  4 

0.0000 

7  j 

0.0000 

1.0000_ 

1.0000_ 

1.0000_ 

1.0000 

(3-17) 


Each  column  matrix  is  a  set  of  coordinates  for  the  corresponding  end-effector 
with  respect  to  the  reference  frame  zero.  The  fourth  element  of  all  vectors  is  equal  to  one. 
This  results  from  Equation  (3-5)  where  a  “one”  is  added  in  order  to  make  w° P  compatible 

to  the  4x4  transformation  matrix.  The  third  element  of  all  vectors  is  equal  to  zero.  This 
is  absolutely  logical  since  there  is  no  motion  in  the  Z-dimension.  The  first  and  the  second 
elements  give  the  coordinates  of  the  corresponding  end-effector  in  the  plane.  If  those  co¬ 
ordinates  are  compared  to  Figure  17,  it  is  possible  to  state  that  the  results  are  fairly  rea¬ 
sonable  and,  therefore,  the  model  works  quite  satisfactorily.  For  example,  it  appears  that 
the  end-effector  is  slightly  above  the  ground  with  y  =  0.0086  meters. 

D.  SUMMARY 

This  chapter  provided  a  brief  description  of  forward  kinematics  theory,  discussed 
robotics  fundamentals  and,  especially,  provided  some  insight  into  manipulator  kinemat¬ 
ics.  In  addition,  a  mathematical  model  for  a  human  manipulator  was  developed.  The  hu¬ 
man  manipulator  described  the  geometry  of  the  lower  limbs  and  the  pelvis.  The  mathe¬ 
matical  model  was  used  to  define  the  position  and  orientation  of  the  end-effector.  Fi¬ 
nally,  a  Matlab  program  was  written  to  demonstrate  the  correctness  of  the  model. 

The  next  chapter  consists  of  two  parts.  The  first  part  presents  the  approaches 
taken  so  far  in  order  to  simulate  human  walking  and  to  estimate  the  position  of  a  person. 
The  second  part  presents  a  computer  simulation  of  human  walking.  Matlab  code  imple¬ 
ments  the  mathematical  model  presented  in  this  chapter,  which  uses  pseudo-generated 
data  to  simulate  a  full  gait-cycle  and  to  calculate  the  distance  walked  along  the  horizontal 
axis. 
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IV.  A  FOUR-SEGMENT  COMPUTER  SIMULATION  OF 
NORMAL  HUMAN  GAIT 

This  chapter  outlines  the  model  that  represents  the  lower  parts  of  the  human  body 
and  that  was  used  in  a  computer  simulation  of  the  normal  human  gait.  This  model  is 
slightly  different  from  the  mathematical  model  derived  in  the  previous  chapter.  It  is  more 
restrictive  so  that  the  movement  can  occur  only  in  the  sagittal  plane.  At  the  beginning  of 
the  chapter,  several  other  approaches  of  computer  simulations  of  human  walking  are  pre¬ 
sented.  The  main  part  of  this  chapter  discusses  the  methodology  as  well  as  the  results  of 
the  simulation. 

A.  APPROACHES  OF  HUMAN  GAIT  SIMULATION 

Two  major  subdivisions  exist  in  order  to  study  the  human  gait:  kinetics  and  kine¬ 
matics.  This  chapter  focuses  on  the  study  of  kinematics  involved  in  human  walking.  For 
any  simulation  to  be  successful,  a  reasonable  kinematic  pattern  should  be  provided.  For 
the  human  gait,  a  simulation  should  include  both  single  and  double  support  phases  [Ref. 
20], 

In  the  past,  several  simulations  have  taken  place  for  many  purposes.  Some  were  to 
achieve  clinical-surgical  goals  [Ref.  21],  while  some  others  have  different  objectives 
[Ref.  22]. 

Another  issue  is  the  interest  that  some  investigators  show  for  a  specific  phase  of 
the  gait  cycle,  or  even  a  specific  subdivision  of  a  phase.  Zarrugh  and  Radcliffe  [Ref.  21] 
have  focused  on  the  simulation  of  the  swing  phase,  while  Henami,  Zheng,  and  Hines 
[Ref.  23]  deal  with  the  initial  position  of  the  body  and,  in  particular,  of  the  lower  limbs 
during  the  initiation  of  walking.  According  to  Gilchrist  and  Winter  [Ref.  20],  dealing 
with  only  one  phase  instead  of  the  entire  gait  cycle  is  more  convenient  in  terms  of  the 
numerical  load  that  occurs  when  the  foot  goes  from  the  swing  phase  to  the  stance  phase 
or,  in  other  words,  from  no  loading  to  full  loading. 

In  some  other  cases,  the  model  which  represents  the  lower  extremities  is  simpli¬ 
fied.  This  translates  to  either  the  representation  of  the  leg  with  one  rigid  bar  instead  of 
two  (shank  and  thigh)  or  a  model  with  no  feet.  It  is  very  common  for  investigators  not  to 
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include  the  foot  segment  in  the  model.  Specifically,  Ju  and  Mansour  [Ref.  24]  introduced 
a  double  limb  model  for  the  leg,  without  the  foot,  in  order  to  simulate  the  support  phase 
of  the  human  gait.  Furthermore,  Amirouche,  Ider,  and  Trimble  [Ref.  25]  followed  the 
same  model  in  their  attempt  to  simulate  and  analyze  human  locomotion.  In  other  experi¬ 
ments,  the  supporting  extremity  consists  of  two  rigid  bars  which  represent  the  thigh  and 
the  shank.  The  foot  is  not  included  while  the  lower  part  of  the  shank  is  fastened  to  the 
ground  [Refs.  26  and  27]. 

Another  major  element  in  these  kinds  of  experiments  is  whether  the  model  is 
two-dimensional  or  three-dimensional.  Individually,  each  limb  undergoes  a  planar 
movement.  However,  both  legs  are  connected  to  each  other  with  the  pelvis.  Since  the 
pelvis  is  perpendicular  to  the  legs,  inevitably,  the  overall  movement  becomes  three- 
dimensional.  This  happens  because  the  pelvis  moves  in  both  the  frontal  and  the  horizontal 
plane.  Some  researchers  prefer  to  disregard  the  pelvis  in  order  to  simplify  their  model, 
which  leaves  a  two-dimensional  model  [Ref.  28].  Apart  from  the  importance  of  the  pel¬ 
vis  as  far  as  the  motion  of  the  lower  part  of  the  body  is  concerned,  it  is  also  important  be¬ 
cause  it  minimizes  the  amount  of  motion  of  the  HAT  (head,  arms,  and  trunk)  during  the 
gait  cycle.  It  is  obvious  that  the  results  are  better  and  far  more  accurate  in  the  case  where 
the  pelvis  is  included  but,  at  the  same  time,  the  computational  load  increases. 

A  very  common  tactic  is  for  the  investigators  to  use  predetermined  trajectories  of 
one  or  more  segments  as  feedback  [Ref.  20].  The  feedback  may  be  a  part  of  a  function 
that  optimizes  the  results.  Davy  and  Audu  use  this  technique  to  predict  muscle  forces  dur¬ 
ing  the  swing  phase  [Ref.  29]. 

Meglan  presented  a  simulation  of  human  walking  that  is  very  close  to  the  ideal 
one.  The  model  consisted  of  segments  representing  the  entire  human  body,  and  not  only 
the  lower  part.  It  is  also  a  three-dimensional  model  and  it  includes  the  feet  [Ref.  30]. 

B.  INTRODUCTION  OF  THE  FOUR-SEGMENT  MODEL 

One  of  the  main  objectives  of  this  thesis  was  to  simulate  human  walking.  This 
was  accomplished  by  creating  a  program  in  Matlab  and  by  implementing  a  model  which 
represents  the  human  body.  The  previous  section  of  this  chapter  presented  several  ap- 
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proaches  concerning  the  simulation  model.  All  of  them  had  advantages  and  drawbacks 
while,  in  many  cases,  the  investigator  had  to  consider  the  tradeoffs  that  appeared. 

In  this  project,  a  computer  simulation  of  the  human  walking  was  developed.  For 
this  purpose,  the  kinematic  approach  was  adopted.  The  kinematic  approach  is  much  sim¬ 
pler  than  the  dynamic  approach,  and  human  movement  can  be  sufficiently  represented 
using  this  kind  of  approach.  According  to  Cheng  and  Moura  [Ref.  31],  there  are  two  basic 
elements  that  have  to  be  considered  before  creating  a  model  for  human  walking.  First, 
there  is  the  model  of  the  human  body  which  gives  the  geometrical  aspect  of  the  problem. 
Second,  there  is  the  model  of  the  walking,  which  provides  the  topological  aspect  of  the 
problem.  These  two  components  are  used  in  order  to  synthesize  the  walker. 

The  human  body  can  be  represented  as  a  multi-link  object  connected  by  joints 
and  rigid  bars.  Models  of  this  kind  vary  from  10  to  17  links.  Each  link  is  connected  to  its 
neighbor  with  a  joint.  The  more  articulated  the  parts  and  degrees  of  freedom,  the  more 
accurately  the  motion  of  the  human  body  is  described.  On  the  other  hand,  this  increases 
the  number  of  the  required  parameters.  Thus,  it  increases  the  complexity  of  the  estimation 
of  human  movement  [Ref.  31].  The  model  used  in  this  project  is  a  model  stick  figure  of 
the  lower  limbs,  and  it  represents  only  the  lower  part  of  the  body.  The  model  shown  in 
Figure  18  consists  of  four  joints  and  joint  angles  0j  (where  i  =  1 ,..,  4  ).  One  link  is  for  the 

right  shank,  one  for  the  right  thigh,  one  for  the  left  shank,  and  one  to  represent  the  left 
thigh. 


Right  leg 


Left  leg 


Height  (m) 


Distance  (m) 


F  igure  18.  S  imulation  Mo  del 
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As  shown  in  Figure  18,  the  model  is  two-dimensional  and  the  motion  of  the  lower 
body  occurs  only  in  the  sagittal  plane.  In  addition,  it  is  clear  that  there  is  no  segment  to 
represent  the  pelvis,  which  is  chosen  to  keep  the  model  in  two  dimensions.  Otherwise, 
due  to  the  limited  but  significant  movement  of  the  pelvis  in  both  the  frontal  and  horizon¬ 
tal  plane,  the  overall  motion  would  be  in  three  dimensions.  This  would  increase  the  com¬ 
putational  load  and  the  number  of  parameters  required  to  describe  the  motion.  Moreover, 
the  model  depicted  in  Figure  18  does  not  include  any  segments  to  represent  the  feet  since 
the  model  used  for  the  simulation  is  nothing  else  but  a  simplified  version  of  the  mathe¬ 
matical  model  derived  in  the  previous  chapter.  That  model  did  not  include  any  segments 
to  represent  the  feet  either.  Hence,  it  is  possible  to  avoid  complex  calculations  and  diffi¬ 
cult  derivations. 

A  brief  discussion  concerning  the  initial  position  of  the  model  is  in  order.  Figure 
19  illustrates  the  initial  position  of  the  model. 


Figure  19.  Initial  Position  of  the  Model 


In  the  initial  position,  the  model  is  at  the  beginning  of  the  stance  phase,  right 
where  the  first  double  support  of  the  gait-cycle  occurs.  Assuming  that  the  reference  leg  is 
the  right  leg,  it  is  fastened  to  the  ground.  If  there  were  feet  included  in  the  model,  then  it 
could  be  claimed  that  this  initial  position  is  described  best  as  a  heel  strike,  which  is  the 
instant  that  the  leading  extremity  strikes  the  ground.  The  traditional  terminology  of  the 
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gait-cycle  introduced  in  Chapter  II,  will  be  applied  in  this  chapter,  in  order  to  better  de¬ 
scribe  the  results  of  the  walking  simulation.  The  lengths  of  the  links  are  constant  as  illus¬ 
trated  in  Table  4.  The  supporting  extremity  touches  the  ground  at  (1,0)  while,  according 
to  the  theory  of  kinematics,  the  coordinates  of  the  joint  as  well  as  the  coordinate  of  the 
end-effector  are  determined  by  the  joint  angles.  Table  4  also  presents  the  initial  set  of 
angles. 


Body  Parts 

Symbols 

Length  in  m 

Joint  angles 

Value  in 
degrees 

Right  Shank 

A 

0.45 

1 

0  |  ^ 

Right  Thigh 

A 

0.4 

1 

|  ^ 

Left  Shank 

A 

0.4 

1 1  n 

12 

Left  Thigh 

A 

0.45 

Determined 
by  9X ,  02 ,  #3 

Table  4.  Lengths  of  Links  and  Values  of  Initial  Angles 

C.  RESULTS  OF  THE  SIMULATION 

After  running  the  program  in  Matlab,  a  window  appears  and  the  model  executes 
the  amount  of  gait-cycles  according  to  the  code.  (This  program  is  illustrated  in  Appendix 
B.)  The  following  five  figures  illustrate  some  characteristic  frames  of  the  movement  of 
the  model.  As  previously  mentioned,  the  coordinates  of  the  model  change  with  respect  to 
the  angles.  Therefore,  since  no  real  data  are  available  yet,  pseudo-data  must  be  intro¬ 
duced  to  move  the  model.  This  pseudo-data  is  based  on  the  theory  of  human  gait-cycle, 
which  was  developed  in  the  previous  chapter.  Their  duration  and  their  subdivisions  and 
the  generation  of  pseudo-data  is  achieved  from  the  theory  of  biomechanics  provided  in 
Chapter  II  regarding  the  phases  of  the  gait-cycle.  Next,  the  gait-cycle  is  divided  into  n 
frames  from  which  approximately  0.6 n  frames  are  used  to  describe  the  stance  phase,  and 
OAn  frames  are  used  to  describe  the  swing  phase. 
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In  Figure  20,  the  model  is  at  stance  phase  and,  more  specifically,  it  is  in  foot  flat 
which  occurs  right  after  the  heel  strike.  By  noticing  that  the  left  leg  has  already  traveled  a 
distance,  it  is  possible  to  claim  that  the  frame  of  Figure  20  depicts  a  moment  between  the 
foot  flat  and  the  midstance. 


Height  (m) 


Figure  20.  Foot  Flat 


In  Figure  21,  the  model  is  at  midstance  at  which  the  body  weight  is  directly  under 
the  supporting  lower  extremity  [Ref.  5].  The  reference  leg  is  still  fastened  to  the  ground. 

In  Figure  22,  the  reference  extremity  is  still  fastened  to  the  ground  but  if  there  was 
a  foot  attached,  this  would  be  the  instant  where  the  heel  of  the  reference  extremity  leaves 
the  ground  or  heel  off.  A  comparison  between  Figure  22  and  the  other  figures  describing 
the  heel  off,  shows  that  this  particular  frame  describes  the  model  as  being  in  the  middle 
of  the  duration  of  the  heel  strike,  just  prior  to  the  extension  of  the  left  leg. 
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Figure  21.  Midstance 
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Figure  22.  Heel  Off 


Figure  23  illustrates  the  instant  when  both  the  lower  extremities  touch  the  ground 
again  (double  support).  This  is  the  last  subdivision  of  the  stance  phase  called  toe  off.  In 
toe  off,  only  the  toe  of  the  reference  extremity  touches  the  ground.  From  that  point  on, 
the  left  leg  will  be  fastened  to  the  ground. 
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Figure  23.  Toe  Off 


In  Figure  24,  the  model  has  entered  the  swing  phase.  In  particular,  it  is  in  accel¬ 
eration  and,  to  be  more  specific,  the  figure  illustrates  the  swinging  extremity  as  directly 
under  the  body. 
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Figure  24.  Acceleration 

Figure  25  illustrates  the  model  in  the  midswing.  This  is  the  moment  when  the 
swinging  extremity  passes  directly  beneath  the  body. 
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Figure  25 


Distance  (m) 

Midswing 


Figure  26  illustrates  the  final  frame  of  the  first  gait-cycle  of  the  model.  It  is  called 
deceleration,  and  occurs  immediately  after  midswing.  Note  that  the  knee  is  extending  and 
it  is  preparing  for  another  heel  strike,  which  will  conclude  the  first  gait-cycle  and  initiate 
the  second. 


Height  (m) 


Figure  26.  Deceleration 
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Figures  27  and  28  illustrate  the  decelerations  for  the  second  and  the  fourth  gait- 
cycle,  respectively. 
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Figure  27.  Deceleration  of  2nd  Gait-Cycle 


Height  (m) 


Figure  28.  Deceleration  of  the  4th  Gait-Cycle 


Another  attribute  of  the  code  is  that  it  can  calculate  the  total  distance  traveled. 
Table  5  illustrates  the  distances  traveled  for  specific  numbers  of  gait-cycles. 
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Number  of  Gait-Cycles 

Total  Distance  Traveled  [m] 

1 

1.3049 

2 

2.6098 

3 

3.9147 

4 

5.2196 

6 

7.8294 

12 

15.6588 

Table  5.  Traveled  Distances 

In  addition,  the  Matlab  code  can  be  modified  to  “command”  the  model  to  move 
with  a  changeable  step.  However,  the  possibilities  of  a  wide  variety  of  step  lengths  and 
flexibility  in  the  motion  patterns  are  rather  limited  due  to  the  strict  rules  of  biomechanics 
implemented  for  the  derivation  of  the  pseudo-data. 

D.  SUMMARY 

Several  approaches  made  by  different  investigators  in  simulating  human  motion, 
and,  in  particular,  human  walking  were  discussed  in  the  beginning  of  this  chapter.  The 
mathematical  model  derived  in  the  previous  chapter  was  simplified  by  subtracting  the 
segment  that  represented  the  pelvis  in  order  to  further  decrease  the  computational  load  as 
well  as  to  limit  the  motion  of  the  model  in  the  sagittal  plane  only.  The  simplified  model 
was  then  implemented  in  a  code  written  in  Matlab.  In  addition,  pseudo-data  was  gener¬ 
ated  to  represent  the  joint  angles,  which  were  a  crucial  parameter  of  the  model  since  a 
kinematics  approach  was  chosen  from  the  beginning.  This  chapter  illustrated  several 
frames  of  the  gait-cycle  of  the  simulation.  The  model  executed  as  many  gait-cycles  as 
the  code  demanded.  Moreover,  the  total  distance  traveled  at  the  end  of  every  gait  cycle 
was  calculated.  Slight  changes  in  the  code  were  made  so  that  the  model  could  achieve  a 
changeable  step. 
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In  the  next  chapter  a  more  sophisticated  model  is  illustrated.  It  consists  of  two 
shanks,  two  thighs,  two  feet  and  a  pelvis.  The  feet  are  mass  rigid  segments  and  modeled 
as  triangles.  In  addition  a  program  in  LISP  is  presented  providing  the  movement  of  a  foot 
also  modeled  as  a  triangle.  Finally  a  comparison  between  the  outcomes  of  those  two  dif¬ 
ferent  approaches  takes  place. 
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V.  ENHANCED  MATLAB  MODEL  AND  COMPARISON  WITH  A 

LISP  MODEL 

In  the  previous  chapter,  a  computer  simulation  of  the  normal  human  gait  was  in¬ 
troduced.  The  simulation  implemented  a  mathematical  model  into  a  Matlab  program, 
which  generated  pseudo-data  to  represent  the  joint  angles.  Many  figures  representing 
several  subdivisions  of  the  gait-cycle  were  illustrated.  In  addition,  another  function  of  the 
Matlab  program  made  the  precise  calculation  of  the  total  distance  traveled  possible. 

This  chapter  presents  a  more  advanced  and  sophisticated  simulation  of  the  human 
gait-cycle.  The  model  consists  of  seven  segments  instead  of  five.  Thus,  this  model  also 
contains  two  feet  and  a  pelvis.  The  main  concept  of  the  simulation  remains  the  same. 
Pseudo-data  of  angles,  generated  from  a  program  in  Matlab  based  on  the  same  principles 
of  biomechanics,  causes  the  movement  of  the  augmented  model. 

Next,  a  simulation  of  the  human  gait-cycle  is  presented  in  LISP.  The  model  used 
consists  of  only  one  foot.  A  program  written  in  LISP  causes  the  foot  to  move,  and  to 
show  its  positions  through  the  various  phases  of  the  gait-cycle.  Finally,  a  comparison  be¬ 
tween  the  two  approaches,  Matlab  and  LISP,  is  provided. 

A.  DESCRIPTION  OF  AN  ENHANCED  MODEL  IN  A  MATLAB 

SIMULATION 

The  simulation  presented  in  the  previous  chapter  managed  to  describe  the  phases 
and  the  subdivisions  of  the  human  gait-cycle  successfully.  However,  it  is  certain  that  the 
kinematic  pattern  would  greatly  improve  with  the  addition  of  a  foot.  As  the  number  of 
segments  that  represents  the  lower  extremities  increases,  so  does  the  number  of  degrees 
of  freedom  and,  consequently,  the  accuracy  of  the  description  of  the  motion  of  the  human 
body.  It  is  obvious  that  in  a  simulation  where  the  feet  are  not  included,  and  the  lower  part 
(shank)  of  the  reference  extremity  is  fastened  to  the  ground,  the  results  do  not  have  the 
accuracy  of  a  simulation  that  implements  a  model  which  contains  the  feet.  The  most  sig¬ 
nificant  problem  that  arises  regarding  the  addition  of  the  feet  is  the  increase  of  the  re¬ 
quired  parameters  and,  therefore,  the  increase  of  the  calculations  required  to  describe  the 
movement  of  the  body. 
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In  addition  to  the  feet,  the  problem  becomes  even  more  complicated  when  a  pelvis 
is  added  to  the  already  existing  model.  Two  issues  must  be  addressed  when  a  pelvis  is 
added.  The  first  issue  is  the  inevitable  increase  of  the  computational  load,  and  the  second 
is  the  manner  in  which  the  movement  of  the  pelvis  will  be  simulated,  given  that  the 
movement  occurs  in  the  sagittal  plane.  Apparently,  the  addition  of  the  pelvis  increases  the 
accuracy  in  describing  the  movement  of  the  body  dramatically.  Furthennore,  the  pelvis 
performs  a  number  of  significant  functions,  such  as  supporting  the  HAT  (head,  arms,  and 
trunk),  lateral  pelvic  tilt,  and  pelvic  rotation  [Ref.  5].  Despite  the  addition  of  the  pelvis, 
the  movement  of  the  model  is  still  described  in  two  dimensions  and  occurs  in  the  sagittal 
plane.  As  mentioned  in  Chapter  III,  during  the  movement  of  the  human  body,  angular 
displacements  of  the  pelvis  occur  in  both  the  X-axis  and  7-axis,  and  reach  their  maxi¬ 
mum  values  ( ±  3  degrees)  at  the  double  support  phase.  Therefore,  in  order  for  the  move¬ 
ment  of  the  pelvis  to  be  accurately  described  in  the  sagittal  plane,  the  projections  of  the 
pelvis  of  the  Z-axis  to  the  X  and  7  axes  are  calculated.  Thus,  the  segment  representing  the 
pelvis  is  not  constant.  It  is  changeable,  and  its  size  and  angles  vary. 

The  lower  extremities  (including  the  pelvis)  are  also  represented  as  a  multi-link 
object,  connected  with  joints  and  rigid  bars.  The  augmented  model  used  in  this  chapter  is 
also  a  model  stick  figure  of  the  lower  limbs.  The  difference  is  that  the  feet  are  not  repre¬ 
sented  by  sticks  but  by  triangles,  in  order  to  describe  the  movement  of  the  feet  and  their 
angular  relationship  to  the  ankles  better.  In  addition,  the  description  of  the  feet  as  trian¬ 
gles  results  in  a  more  realistic  simulation,  since  they  are  considered  as  mass  rigid  seg¬ 
ments.  Therefore,  the  augmented  model  consists  of  five  links  and  two  triangles.  Two 
links  represent  the  shanks,  two  links  represent  the  thighs,  one  link  with  changeable  length 
represents  the  pelvis  and,  finally,  two  triangles  represent  the  mass  rigid  feet.  Figure  29 
illustrates  the  initial  position  of  the  augmented  model. 
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Figure  29.  Initial  Position  of  the  Augmented  Model 


As  shown  in  Figure  29,  the  augmented  model  is  still  two  dimensional  with  the  dif¬ 
ference  that  the  pelvis  is  represented  by  one  link  of  changeable  length.  At  this  point  of 
double  support,  the  length  of  the  projection  of  the  pelvis  reaches  its  maximum  value.  The 
initial  position  of  the  augmented  model  is  at  the  beginning  of  the  stance  phase.  Assuming 
that  the  reference  leg  is  the  right  one  (red-colored),  the  initial  position  occurs  right  when 
the  leading  extremity  strikes  the  ground  (heel  strike). 

B.  RESULTS  OF  THE  SIMULATION 

Following  the  exact  same  procedure  as  in  the  previous  chapter,  the  augmented 
model  executes  the  movement  according  to  the  Matlab  program.  (The  program  for  the 
simulation  presented  in  this  chapter  is  illustrated  in  Appendix  C.)  The  following  figures 
illustrate  some  characteristic  frames  of  the  movement  of  the  model. 

In  Figure  30,  the  model  is  in  foot  flat.  In  the  previous  chapter,  this  could  only  be 
estimated,  since  there  was  no  segment  to  represent  the  foot.  However,  in  this  case,  the 
figure  verifies  the  given  description  since  the  foot  of  the  supporting  extremity  is  in  full 
contact  to  the  ground.  Furthermore,  the  length  of  the  projection  of  the  pelvis  is  decreased. 

In  Figure  3 1,  the  augmented  model  is  in  midstance  and,  as  shown  in  the  figure, 
the  pelvis  is  almost  vertical  to  the  sagittal  plane. 
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Figure  3 1 .  Midstance 

In  Figure  32,  the  heel  of  the  reference  extremity  leaves  the  ground.  This  is  called 
heel  off.  In  the  previous  chapter,  the  reference  extremity  was  fastened  to  the  ground  and, 
therefore,  the  heel  off  phase  was  approximated.  Using  the  augmented  model  makes  it 
possible  to  observe  the  exact  moment  where  each  phase  occurs.  Furthermore,  the  projec¬ 
tion  of  the  pelvis  begins  to  increase  again. 
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Figure  30.  Foot  Flat 
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Figure  32.  Fleel  Off 

In  Figure  33,  a  double  support  phase  occurs  for  the  second  time.  This  is  the  mo¬ 
ment  when  the  reference  extremity  changes  from  one  leg  to  another.  The  toe  of  the  previ¬ 
ous  reference  extremity  is  ready  to  leave  the  ground,  and  the  heel  of  the  new  reference 
extremity  strikes  the  ground.  This  phase  is  called  toe  off.  The  length  of  the  projection  of 
the  pelvis  reaches  its  maximum  value  once  more. 


Figure  33.  Toe  Off 
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In  Figure  34,  the  augmented  model  is  in  the  swing  phase.  The  foot  of  the  support¬ 
ing  extremity  is  in  full  contact  to  the  ground,  while  the  swinging  extremity  is  exactly  un¬ 
der  the  body. 


1.8 
1.6 
1.4 
1.2 

Height  (m)  1 

0.8 
0.6 
0.4 
0.2 
0 

-0.2 
0 

Distance  (m) 

Figure  34.  Acceleration 

In  Figure  35,  the  midswing  occurs.  In  midswing,  the  swinging  extremity  passes  di¬ 
rectly  beneath  the  body. 


Figure  35.  Midswing 
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In  Figure  36,  the  last  step  of  the  gait-cycle  occurs.  It  is  called  deceleration  and  it 
describes  the  moment  where  the  knee  of  the  swinging  extremity  is  extending,  and  the 
heel  is  ready  to  strike  the  ground  and  initiate  a  new  gait-cycle. 


Figure  36.  Deceleration 

This  section  presented  a  more  sophisticated  simulation  of  the  gait-cycle.  The 
model  consisted  of  seven  segments.  Two  thighs,  two  shanks,  two  feet,  and  a  pelvis.  Next, 
a  simulation  of  the  human  gait-cycle  is  presented  in  LISP. 

C.  A  DIFFERENT  APPROACH  USING  LISP 

Another  program  written  in  LISP  describes  the  movement  of  the  foot  during  the 
human  gait-cycle.  LISP  was  chosen  from  among  the  other  computer  languages  for  this 
approach  because  it  still  is  a  versatile  language  and  can  be  successfully  applied  to  com¬ 
puter  simulation  of  physical  systems  such  as  the  human  body  [Ref.  32].  Professor  R. 
McGhee  provided  the  LISP  program,  which  is  illustrated  in  Appendix  D. 

The  following  figures  illustrate  the  position  of  the  foot  during  the  gait-cycle. 
More  specifically,  Figure  37  depicts  the  foot  in  heel  strike,  Figure  38  refers  to  foot  flat, 
and  Figure  39  illustrates  the  toe  off 
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Figure  37.  Heel  Strike 
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Figure  38.  Foot  Flat 
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Figure  39.  Toe  Off 
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Toe  off  is  the  point  where  the  foot  leaves  the  ground  and  it  passes  from  the  stan  ce 
phase  to  the  swing  phase.  Therefore,  Figures  40,  41,  and  42  illustrate  the  acceleration, 
midswing,  and  deceleration,  respectively. 
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Figure  40.  Acceleration 
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F igure  4 1 .  Midswing 
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Figure  42.  Deceleration 

Immediately  after  the  deceleration  phase  depicted  in  Figure  42,  the  gait-cycle 
starts  from  the  beginning.  This  program  was  written  in  such  a  way  so  that  two  or  more 
consecutives  gait-cycles  could  be  provided.  Figure  43  illustrates  a  two-gait-cycle 
movement,  and  Figure  44  depicts  a  three-gait-cycle  movement. 
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Figure  43.  Two-Gait-Cycle  Movement 
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Next,  a  brief  comparison  between  the  two  methods  is  in  order.  Both  methods  de¬ 
scribe  the  human  motion  and,  particularly,  the  movement  of  the  lower  extremities.  How¬ 
ever,  with  the  first  approach,  the  movement  of  the  entire  lower  human  body  is  under 
study:  the  behavior  of  the  pelvis  in  the  sagittal  plane,  the  relationships  between  the 
shanks  and  the  thighs,  and  the  way  that  the  mass  rigid  feet  are  connected  to  the  ankles. 
With  the  second  approach  (LISP),  only  the  movement  of  one  foot  is  under  study.  Fur¬ 
thermore,  the  Matlab  program  allows  the  model  to  move  contiguously  towards  a  certain 
direction.  After  compiling  the  program  a  window  appears,  and  the  model  executes  the 
gait-cycle  walking  towards  the  X-axis.  This  does  not  happen  with  LISP  where  the  pres¬ 
entation  of  the  gait-cycle  is  discrete.  Thus,  for  each  phase  of  the  foot  to  be  presented,  a 
different  command  is  given. 

On  the  other  hand,  Matlab  programs  are  considered  rather  difficult  for  someone  to 
understand  and  even  more  difficult  to  intervene,  compared  to  LISP  programs,  which  are 
relatively  easy  to  read  since  it  has  a  free  coding  form  and  a  simple  syntax.  Hence,  it  is 
more  like  reading  a  foreign  language  than  a  computer  language  [Ref.  32]. 

D.  SUMMARY 

This  chapter  discussed  the  implementation  of  an  enhanced  model  to  simulate  the 
human  gait-cycle.  The  differences  in  this  model  compared  to  that  described  in  the  previ- 
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ous  chapter  were  the  addition  of  the  pelvis,  and  the  addition  of  one  mass  rigid  foot,  de¬ 
scribed  as  a  triangle,  to  each  leg.  Several  figures  from  various  phases  of  the  simulation 
were  illustrated. 

Next,  another  simulation  was  provided  by  a  program  written  in  LISP.  The  model 
consisted  of  only  one  mass  rigid  foot,  described  as  a  triangle,  and  the  phases  of  the  gait- 
cycle  as  well  as  the  outcomes  of  more  than  one  gait-cycle  were  presented  in  several  fig¬ 
ures.  Finally,  a  brief  comparison  between  the  two  approaches  followed. 

The  next  chapter  presents  the  concepts  of  motion  tracking  and  position  estimation. 
The  MARG  sensors  were  used  to  derive  real  data  and  to  incorporate  them  into  the  Matlab 
program  developed  earlier  in  this  project  to  describe  a  human  walking. 
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VI.  TESTING  OF  THE  SIMULATION  MODEL  USING  MARG 

SENSOR  DATA 


The  previous  chapter  presented  an  advanced  simulation  of  the  human  gait-cycle. 
An  augmented  model  was  implemented  for  that  purpose.  In  addition  to  the  simple  model 
demonstrated  in  Chapter  IV,  the  advanced  model  consisted  of  two  feet  and  a  pelvis.  Thus, 
a  more  realistic  representation  of  human  walking  was  illustrated.  Furthermore,  the  simu¬ 
lation  for  the  advanced  model  was  compared  to  a  second  simulation,  in  which  a  LISP 
program  was  used  to  represent  the  phases  of  a  single  foot  during  the  gait-cycle. 

This  chapter  discusses  the  concepts  of  motion  tracking  and  position  estimation. 
Next,  the  implementation  of  the  MARG  sensors  to  efficiently  describe  the  motion  of  the 
lower  extremities  is  discussed.  Real  data  is  derived  and  used  in  the  existing  Matlab  pro¬ 
gram  to  describe  the  human  gait-cycle  and  to  estimate  the  position  of  the  model. 

A.  MOTION  TRACKING  AND  POSITION  ESTIMATION 

Tracking  human  motion  and,  especially,  human  walking  has  been  the  field  of 
study  for  many  researchers.  Several  techniques  have  been  developed  for  that  purpose  and 
quite  a  few  papers  have  been  written.  The  following  three  steps  describe  the  most  con¬ 
ventional  approach  of  tracking  human  motion  [Ref.  33]: 

•  Derivation  of  real  data  from  real  human  motion  by  using  three- 
dimensional  motion-capturing  systems  such  as  cameras,  etc. 

•  Simulation  of  human  motion,  using  theoretical  approaches  such  as  forward 
or  inverse  kinematics/dynamics. 

•  Interpolation  between  frames. 

As  is  known,  the  complex  nature  of  the  human  body  does  not  allow  accurate  hu¬ 
man  motion  tracking  based  on  relatively  simple  models.  There  is  a  tradeoff  between  the 
accuracy  of  the  result  and  the  simplicity  of  the  model  used  for  that  purpose.  Most  re¬ 
searchers  prefer  to  choose  simple  models  rather  than  deal  with  complex  algorithms  that 
increase  the  computational  load  exponentially,  although  current  computer  capabilities 
allow  the  manipulation  of  an  extreme  amount  of  data.  The  most  common  model  is  that  of 
each  limb  represented  by  a  rigid  bar  connected  to  its  neighbors  with  a  joint.  Jia-Ching 
Cheng  and  Jose  M.  F.  Moura  [Ref.  17]  track  human  walking  using  such  a  model.  The 

model  was  given  real  data  derived  from  a  video  sequence.  A  similar  procedure  was  pre- 
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sented  by  A.  G.  Bharatkumar,  K.  E.  Daigle,  M.  G.  Pandy,  Qin  Cai  and  J.  K.  Aggarwal 
[Ref.  15].  Real  data  was  obtained  from  people  walking  with  markers  on  their  limbs.  Next, 
the  real  data  is  provided  to  a  model  similar  to  that  described  above. 

Position  estimation  is  another  issue  and  it  is  not  necessarily  connected  to  tracking 
human  motion  in  detail.  Steven  Feiner,  Blair  McIntyre,  Tobias  Hollerer,  and  Anthony 
Webster  [Ref.  34]  achieved  a  satisfactory  estimation  of  position  by  using  a  digital  GPS, 
magnetometers,  and  an  inertial  head  tracker.  However,  this  is  good  only  for  outside  envi¬ 
ronments.  For  indoors  environments,  this  configuration  is  rather  inadequate  due  to  the 
low  signal  level  of  the  GPS.  Masakatsu  Kurogi  and  Takeshi  Kurata  [Ref.  35]  proposed  a 
method  of  personal  positioning,  which  allows  both  indoor  and  outdoor  motion.  This 
method  is  based  on  the  estimates  of  relative  displacement,  which  used  the  analysis  of 
human  walking  behavior  using  self-contained  sensors.  The  method  is  also  based  on  the 
estimates  of  position  and  orientation  within  a  Kalman  filtering  framework. 

However,  a  different  approach  in  tracking  human  motion  and,  especially,  human 
walking  and  estimating  the  position  is  proposed  in  this  thesis.  The  positions  of  the  limbs 
of  the  lower  extremities  can  be  estimated  using  the  MARG  sensors.  They  are  small  iner¬ 
tial/magnetic  sensors,  consisting  of  three  accelerometers,  three  magnetometers,  and  three 
angular  rate  sensors.  All  three  triads  are  orthogonally  mounted  [Ref.  36].  The  sensors’ 
data  is  processed  by  a  filtering  algorithm  capable  of  tracking  in  all  orientations.  The  set  of 
angles  provided  by  the  algorithm  is  used  in  the  Matlab  program,  presented  in  previous 
chapters,  in  order  to  describe  human  walking.  Therefore,  the  real  data  provided  by  the 
sensors  is  implemented  in  the  program  instead  of  the  pseudo-data.  Moreover,  the  total 
distance  traveled  can  also  be  calculated.  Since  the  motion  occurs  in  the  sagittal  plane,  the 
tenn  position  estimation  refers  to  the  total  distance  traveled  along  one  axis. 

B.  EQUIPMENT  SETUP 

The  data  acquisition  was  achieved  by  using  the  magnetic,  angular  rate  and  gravity 
(MARG)  sensors.  Data  from  the  sensors  is  sent  to  the  control  interface  unit  (CIU)  and 
then  transmitted  through  a  wireless  system.  Andreas  Kavousanos-Kavousanakis  [Ref.  2] 
thoroughly  described,  tested,  and  evaluated  this  configuration.  Therefore,  only  a  brief 
summary  of  each  part  is  provided  in  this  thesis.  Even  though  MARG  IV  sensors  were 

available,  due  to  some  technical  difficulties  concerning  communication  problems  be- 
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tween  the  sensors  themselves  and  the  new  16-channel  CIU,  the  previous  model  of 
MARG  III  sensors  had  to  be  used.  The  sensor  consists  of  three  magnetometers,  three  ac¬ 
celerometers,  three  angular  rate  sensors,  and  a  microcontroller.  Figure  45  illustrates  a 
MARG  III  sensor. 


Figure  45.  The  MARG  III  Sensor  [From  Ref.  2.] 


The  data  obtained  from  the  sensors  must  be  transmitted  wirelessly  to  the  central 
terminal  to  be  further  processed.  Every  sensor  has  nine  elements  with  data  flowing  from 
each  one  of  them.  Therefore,  for  this  kind  of  data  to  be  sent  across  through  a  single  chan¬ 
nel,  it  must  be  multiplexed.  This  is  the  task  of  the  CIU.  The  first  CIU  was  a  one-channel 
CIU.  However,  for  the  purposes  of  this  thesis,  a  three-channel  CIU  was  used,  which  is 
nothing  more  than  three  one-channel  CIUs  in  a  parallel  configuration.  Figure  46  illus¬ 
trates  a  three-channel  CIU. 
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Figure  46.  The  Three-Channel  CIU  [From  Ref.  2.] 


During  the  time  that  this  thesis  was  written,  the  16-channel  CIU  was  available. 
This  CIU  is  capable  of  multiplexing  the  data  from  16  different  sensors  and  deliver  it 
through  a  single  communication  channel  to  the  terminal.  However,  due  to  the  nature  of 
the  research,  the  use  of  this  particular  CIU  was  not  necessary.  The  use  of  the  three- 
channel  CIU  proved  to  be  more  than  adequate. 

C.  PROCEDURE/RESULTS 

As  mentioned  previously,  the  sensors  generate  data  which  were  delivered  to  the 
CIU.  In  the  CIU,  the  data  is  multiplexed  in  order  to  be  transmitted  wirelessly  through  a 
single  channel  to  the  terminal.  The  data  which  reach  the  terminal  are  neither  understand¬ 
able  nor  easy  to  use  directly  in  other  applications.  Table  6  illustrates  some  portion  of  the 
“raw”  data  delivered  from  the  CIU. 


0  1  1570  1549  1500  1349  2169  1875  1903  1619  1344 

0  2  1569  1548  1499  1349  2166  1876  1901  1620  1345 

0  3  1569  1550  1500  1351  2170  1873  1903  1620  1345 

0  4  1569  1549  1500  1350  2170  1874  1902  1619  1346 

0  5  1569  1548  1500  1351  2171  1872  1903  1619  1345 

0  6  1569  1548  1500  1353  2171  1872  1904  1620  1345 

0  7  1569  1549  1500  1352  2174  1872  1902  1619  1344 

0  8  1569  1549  1500  1353  2172  1872  1902  1620  1344 

0  9  1568  1548  1500  1352  2171  1872  1902  1619  1345 

0  10  1569  1548  1500  1353  2170  1874  1901  1619  1345 

0  11  1570  1548  1500  1351  2170  1872  1902  1620  1346 

0  12  1570  1550  1500  1353  2171  1871  1903  1619  1345 

0  13  1570  1549  1500  1354  2169  1872  1902  16[L8  1345 

0  14  1570  1548  1500  1351  2170  1871  1902  1617  1347 

0  15  1570  1549  1500  1353  2168  1873  1902  1619  1345 

0  16  1570  1549  1500  1352  2168  1875  1902  1618  1345 

0  17  1569  1548  1500  1352  2172  1875  1903  1618  1345 

0  18  1569  1548  1501  1348  2168  1875  1902  1618  1344 

0  19  1568  1549  1500  1352  2169  1873  1901  1618  1345 

0  20  1569  1549  1500  1351  2170  1872  1903  1619  1344 


Table  6.  Data  Delivered  from  the  CIU. 
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The  first  column  represents  the  time  in  seconds  when  the  specific  measurement 
occurs  (e.g.,  at  zero  seconds  in  Table  6).  The  second  column  is  the  number  of  the  sample. 
There  are  100  samples  taken  per  second.  The  following  nine  columns  represent  the  triads 
of  the  magnetometers,  the  accelerometers,  and  the  angular  rate  sensors,  respectively. 

The  “raw”  data  must  be  transfonned.  For  that  purpose,  Andreas  Kavousanos- 
Kavousanakis  [Ref.  2]  proposed  the  Quest  algorithm,  which  used  quaternions  to  calculate 
the  orientation  of  the  limb  on  which  the  sensor  was  attached.  However,  this  method  did 
not  use  the  measurements  of  the  angular  rate  sensors,  which  resulted  in  an  inadequate 
representation  of  the  human  motion  during  large  accelerations.  To  fix  this  problem,  Con- 
rado  Aparicio  [Ref.  3]  presented  the  Factored  Quaternion  algorithm.  A  Kalman  filter  was 
implemented  and  combined  the  static  estimated  quaternion  with  the  measurements  from 
the  angular  rate  sensors  to  provide  the  optimal  orientation  estimate.  Both  Andreas 
Kavousanos-Kavousanakis  and  Conrado  Aparicio  managed  to  track  the  motion  of  the 
right  arm  in  real  time. 

However,  the  main  objective  of  this  thesis  was  not  to  track  the  motion  of  the 
lower  extremities  in  real  time.  The  goal  was  to  gather  data  from  the  sensors  and  to  incor¬ 
porate  it  into  the  existing  Matlab’s  simulation  program  to  move  the  model.  As  mentioned 
previously,  the  three-channel  CIU  is  used  for  processing  the  data  obtained  from  the  sen¬ 
sor.  Another  issue  is  that  the  existing  software  does  not  allow  taking  data  from  more  than 
one  sensor  at  the  same  time.  Thus  only  one  sensor  is  used.  For  example,  the  sensor  was 
attached  to  the  thigh  and  after  running  the  program,  data  were  obtained  and  saved  in  a 
text  file  to  be  loaded  in  the  Matlab  program.  Next,  the  sensor  was  attached  to  the  shank 
and  the  same  procedure  was  followed.  This,  of  course,  creates  an  additional  difficulty, 
since  the  samples  from  the  two  measurements  to  be  incorporated  into  the  Matlab  program 
must  be  as  highly  correlated  as  possible. 

The  “raw”  data  delivered  from  the  CIU  is  transformed  into  quaternions  and  then 
into  angles  by  using  the  Factored  Quaternion  algorithm.  Table  7  illustrates  the  quater¬ 
nion  measurements  of  a  sensor  attached  to  the  right  thigh,  while  the  person  is  in  the 
stand-up  position.  In  the  table,  the  first  10  samples  of  the  experiment  are  shown. 
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Sample 

?0 

d. 

l 

0.29198190246990 

0.66565006440052 

0.30672889578596 

0.61447045891854 

2 

0.29292186055944 

0.66443412124903 

0.30896746098712 

0.61421754303960 

3 

0.29410101634827 

0.66505770870310 

0.30936581566533 

0.61277698094431 

4. 

0.29514747448117 

0.66518018298469 

0.30958597513655 

0.61202926112286 

5 

0.29590692455467 

0.66564386890250 

0.30970296314723 

0.61109852430761 

6 

0.29672858201358 

0.66595294873036 

0.31003756145395 

0.61019302616568 

7 

0.29747698061030 

0.66644882946701 

0.30977408729708 

0.60942055966984 

8 

0.29820349632252 

0.66664910321432 

0.30994922099631 

0.60875703558016 

9 

0.29887764092890 

0.66675166731840 

0.31025910833958 

0.60815594675481 

10 

0.29944922658029 

0.66677150254690 

0.31053376802901 

0.60771268129398 

Table  7.  Quaternion  Measurements  for  Sensor  Attached  to  Right  Thigh  in  the 

Stand-Up  Position. 

The  Factored  Quaternion  algorithm  also  transforms  the  quaternions  into  angles. 
Table  8  illustrates  the  angle  measurements  of  a  sensor  that  is  attached  to  the  right  thigh, 
while  the  person  is  in  the  stand-up  position.  More  specifically,  the  first  10  samples  of  the 
experiment  are  shown. 


Sample 

A-axis  (Roll) 

F-axis  (Pitch) 

Z-axis  (Yaw) 

l 

171.008478205228 

85.683252083344 

41.139346835021 

2 

172.512741690264 

85.727367806572 

42.927526774052 

3 

170.997251482504 

85.585802676671 

41.555987864367 

4 

169.861075130236 

85.536055177006 

40.534336684944 

5 

168.817169040274 

85.439891718533 

39.570874573482 

6 

167.994819515943 

85.353669467500 

38.853972564782 

7 

166.716323701608 

85.271536035443 

37.620624398289 

8 

165.975826477322 

85.212126041704 

36.962298928267 

9 

165.454142783164 

85.161905494336 

36.530052334811 

10 

165.053655385336 

85.129530808321 

36.206296216028 

Table  8.  Angle  Measurements  for  Sensor  Attached  to  Right  Thigh  in  the  Stand-Up 

Position. 
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At  this  point,  the  following  question  arises.  Since  the  person  is  standing  up  and 
facing  the  magnetic  north  during  the  measurement,  what  is  the  reason  for  the  deviation  of 
the  measured  values  from  the  theoretical  ones?  For  example,  the  roll  value  is  approxi¬ 
mately  171  degrees  instead  of  180  and  the  pitch  is  85.7  degrees  instead  of  the  theoretical 
90.  One  reason  is  definitely  human  error  in  aligning  the  sensor  to  the  magnetic  north. 
Another  reason  is  the  slight  miscalculations  that  might  have  occurred  during  the  initial 
calibration  of  the  sensor.  However,  after  conducting  several  tests  in  which  the  axes  of  the 
sensor  were  completely  aligned  to  the  Earth’s  fixed  coordinate  system,  the  following  re¬ 
marks  are  provided. 

The  measurements  for  the  roll  vary  from  -4  to  4  degrees  with  zero  being  the  theo¬ 
retical  value.  Similarly,  the  measurements  for  the  pitch  vary  from  4  to  9  degrees  with 
zero  being  the  theoretical  value.  Finally,  the  measurements  for  the  yaw  vary  from  -4  to 
-8  degrees  with  zero  being  the  theoretical  value.  Those  deviations  are  taken  into  consid¬ 
eration  during  the  phase  where  exhaustive  tests  are  performed  to  collect  data  for  the  Mat- 
lab  program.  However,  the  most  important  reason  is  that  in  an  indoor  environment,  the 
various  electromagnetic  fields  affect  the  magnetometers  of  the  sensors  resulting  in  this 
rather  significant  deviation. 

During  the  experiments,  only  one  sensor  was  utilized.  The  sensor  was  already 
calibrated  and  attached  in  such  a  way  so  that  the  7-axis  would  point  to  the  magnetic 
north.  Furthermore,  the  X-axis  would  point  to  the  head  of  the  model  and  the  Z-axis 
would  be  consistent  with  the  right  hand  rule.  Therefore,  in  the  simulation,  the  model  is 
moving  towards  the  7-axis.  The  sensor  was  first  attached  to  the  supporting  thigh,  then  to 
the  supporting  shank,  then  to  the  swinging  thigh,  and  finally  to  the  swinging  shank. 
Measurements  were  taken  separately  from  each  limb  and  many  sets  of  real  data  were 
saved  to  be  implemented  to  the  simulation  program.  The  initial  position  of  the  model,  as 
illustrated  in  Figure  47,  depicts  the  supporting  limbs  in  the  toe  off  position  and  the  swing¬ 
ing  limbs  in  the  foot  flat  position. 
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1 


Swinging 


Distance  (m) 

Figure  47.  Initial  Position  of  the  Model  during  the  Real  Data  Simulation 

Since  only  one  sensor  could  be  used  at  each  time,  the  movement  of  the  model  had 
to  be  exactly  the  same  for  the  extracted  sets  of  data  to  be  consistent.  However,  it  is  more 
than  obvious  from  the  simulation  that,  no  matter  how  well  correlated  are  the  data,  it  will 
never  have  as  satisfactory  results  as  when  four  individual  sensors  are  attached  to  the  four 
limbs  and  the  data  is  processed  simultaneously. 

The  Matlab  program,  provided  in  Appendix  E,  consists  of  two  parts.  The  first  part 
is  the  Factored  Quaternion  algorithm,  which  derives  the  values  of  the  angles  in  all  direc¬ 
tions.  The  second  part  implements  the  angles  in  a  simulation  to  track  the  motion  of  the 
lower  extremities.  Figure  48  illustrates  the  final  position  of  the  model.  The  model  is  at  the 
double  support  phase. 
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Figure  48.  Final  Position  of  the  Model  in  Real  Data  Simulation 


During  the  experiments,  the  actual  length  of  the  step  was  measured  and  found  to 
be  84  cm.  The  program  is  not  only  able  to  track  the  motion  of  the  lower  extremities,  but 
to  estimate  the  position  of  the  model  as  well.  Since  the  motion  of  the  model  occurs  in  the 
sagittal  plane,  the  estimation  of  the  position  of  the  model  automatically  translates  to  the 
estimation  of  the  distance  that  the  model  has  traveled  in  the  7-axis.  From  the  experiments 
conducted,  the  mean  value  of  the  step  length  was  found  equal  to  60  cm.  However,  if  the 
length  of  the  foot  is  also  added,  then  the  length  of  the  step  reaches  82  cm,  which  agrees 
with  the  estimated  value. 

The  following  remark  deals  with  the  position  of  the  model  when  it  moves  in  three 
dimensions  and  it  might  be  useful  information  for  moving  this  project  to  another  level. 

As  mentioned  previously,  the  7-axis  of  the  attached  sensor  points  to  the  magnetic  north. 
Th e  yaw  angle  is  equal  to  90  degrees.  When  the  sensor  is  turned  towards  East,  the  yaw 
angle  goes  all  the  way  up  to  180  degrees.  On  the  other  hand,  when  the  sensor  is  turned 
towards  West,  th q  yaw  angle  goes  all  the  way  up  to  zero  degrees.  Therefore,  if  the  simu¬ 
lation  program  is  modified  so  that  the  model  undergoes  a  three-dimension  movement,  the 
transition  from  the  sagittal  to  the  frontal  plane  and  vice  versa,  could  be  identified  by  the 
values  of  the  yaw  angles. 
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D.  SUMMARY 

This  chapter  discussed  the  concepts  of  tracking  human  motion  and  position  esti¬ 
mation.  Next,  the  implementation  of  the  MARG  III  sensors  in  tracking  the  motion  of  the 
lower  extremities  and  estimating  the  position  of  the  model  is  discussed.  Furthermore,  the 
equipment  utilized  in  this  project  was  presented,  along  with  a  brief  description.  Finally, 
the  procedure  for  processing  the  data  from  the  sensors  for  the  simulation  program  was 
discussed  and  the  results  of  the  simulation,  where  real  data  were  used,  were  illustrated. 

The  next  chapter  presents  the  conclusions  and  the  contributions  of  this  thesis.  It 
also  provides  suggestions  for  further  improvements  and  future  work. 
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VII.  CONCLUSIONS  AND  FUTURE  WORK 


A.  CONCLUSIONS 

This  thesis  was  able  to  track  the  motion  of  the  lower  extremities  of  a  human  and 
to  estimate  the  traveled  distance.  The  MARG  sensors  were  used  to  track  the  motion  of  the 
lower  extremities  and  estimate  the  position  of  the  model.  The  lower  extremities  were 
modeled  as  an  articulated  object,  which  consisted  of  rigid  bars  connected  to  each  other  by 
joints. 

The  goal  of  this  thesis  was  accomplished  by  incorporating  real  data  into  a  com¬ 
puter  simulation  written  in  Matlab.  The  simulation  model  was  initially  driven  by  pseudo¬ 
data.  The  pseudo-data  was  nothing  more  than  a  set  of  angles  which,  after  being  given  an 
initial  value,  were  incremented  or  decremented,  depending  on  the  limb  and  on  the  phase 
of  the  gait-cycle.  To  accurately  simulate  human  walking,  the  basic  principles  of  biome¬ 
chanics  as  well  as  the  phases  of  the  gait-cycle  had  to  be  taken  into  consideration  in  the 
formation  of  the  pseudo-data.  Furthermore,  a  mathematical  model,  based  on  the  theory  of 
forward  kinematics  and  the  theory  of  manipulators  kinematics,  was  derived.  This  model 
formed  the  main  part  of  the  Matlab  program,  and  the  pseudo-data,  as  well  as  the  real 
data,  were  incorporated  directly  in  it. 

The  real  data  was  derived  from  a  magnetic,  angular  rate,  and  gravity  (MARG) 
sensor.  For  the  purposes  of  this  thesis,  a  MARG  III  sensor  was  used.  The  data  derived 
from  the  sensor  was  processed  by  a  control  interface  unit  (CIU).  The  CIU  multiplexed  the 
data  so  that  it  could  go  through  a  single  channel  and  be  transmitted  wirelessly  to  the  cen¬ 
tral  tenninal.  Next,  it  was  further  processed  by  the  Factored  Quaternion  algorithm,  which 
transformed  the  “raw”  data  into  quaternions  and  then  into  angles,  calculating  the  orienta¬ 
tion  in  every  direction.  The  set  of  angles  was  provided  to  the  simulation  model,  which 
was  then  able  to  track  the  motion  of  the  lower  limbs  efficiently. 

The  existing  software  did  not  allow  the  incorporation  of  data  from  more  than  one 
sensor  at  the  same  time.  Therefore,  for  example,  in  order  to  track  the  motion  of  one  leg 
consisting  of  a  thigh  and  a  shank,  the  sensor  first  had  to  be  attached  to  the  thigh,  and  pro¬ 
vide  the  data  related  to  the  motion  of  the  thigh.  Next,  it  had  to  be  attached  to  the  shank 
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and  provide  the  data  related  to  the  motion  of  the  shank.  Obviously,  the  outcome  was  not 
as  good  as  it  would  have  been  if  the  measurements  had  been  taken  from  both  locations  at 
the  same  time.  In  order  to  overcome  that  difficulty  and  to  provide  a  result  that  would  be 
as  realistic  as  possible,  extensive  tests  had  to  be  conducted.  The  measurements  that  pro¬ 
vided  the  best  correlation  between  the  thigh  data  and  the  shank  data  were  incorporated 
into  the  Matlab  simulation  program.  The  estimation  of  the  position  of  the  model  in  a  two- 
dimensional  environment  translates  to  the  estimation  of  the  distance  that  the  model  has 
traveled  along  one  axis.  From  the  experiments  conducted,  the  mean  value  of  the  step 
length  was  found  equal  to  82  cm,  which  agrees  with  the  estimated  value  (84  cm). 

Another  important  remark  is  that  this  thesis  did  not  deal  with  real  time  human  mo¬ 
tion  tracking.  The  objective  of  this  thesis  was  to  study  the  implementation  of  the  MARG 
III  sensors  in  tracking  the  motion  of  the  lower  extremities  by  incorporating  the  pseudo¬ 
data  or  the  data  obtained  from  the  sensors  into  a  simulation  program  written  in  Matlab. 
There  was  no  implementation  of  any  algorithm  in  Java  whatsoever,  unlike  some  of  the 
references. 

In  addition,  the  various  measurements  of  the  sensors  showed  that  a  significant  de¬ 
viation  existed  between  the  measured  values  of  the  angles  and  the  theoretical  values,  pos¬ 
sibly  due  to  errors  during  the  original  calibration.  However,  the  existence  of  various  elec¬ 
tromagnetic  fields,  which  definitely  affected  the  magnetometers  of  the  sensors,  seems  to 
be  a  more  likely  explanation  for  this  deviation. 

B.  FUTURE  WORK 

This  thesis  achieved  the  tracking  of  the  motion  of  the  lower  extremities.  However, 
to  reach  a  point  to  which  all  of  this  can  be  implemented  in  a  virtual  environment,  much 
more  remains  to  be  done. 

The  possibility  of  obtaining  data  from  more  than  one  sensor  is  absolutely  essential 
for  the  completion  of  the  MARG  project.  As  far  as  the  lower  extremities  are  concerned, 
the  utilization  of  six  sensors  would  be  enough.  Two  of  them  would  be  attached  on  the 
thighs,  two  on  the  shanks,  and  two  on  the  feet.  The  derivation  of  data  from  all  the  sensors 
at  the  same  time  would  prevent  the  researchers  from  conducting  exhaustive  experiments 
to  achieve  the  best  correlation  between  the  data. 
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Another  issue  that  must  be  addressed  is  the  communication  problems  between  the 
MARG  IV  sensors  and  the  16-channel  CIU  that  appeared  during  the  research.  The  utili¬ 
zation  of  the  16-channel  CIU  will  allow  the  depiction  of  the  motion  of  all  the  limbs  to 
which  a  sensor  is  attached. 

The  simulation  model  used  in  this  thesis  restricted  movement  to  the  sagittal  plane. 
Consequently,  the  position  estimation  of  the  model  refers  to  the  estimation  of  the  distance 
that  the  model  traveled  in  one  direction.  The  next  step  is  to  move  this  concept  to  a  higher 
level  by  developing  a  model  that  would  move  in  three  dimensions  instead  of  two.  Hence, 
the  term  position  estimation  would  refer  to  the  position  of  the  model  in  an  extended  hori¬ 
zontal  plane  as  defined  in  Chapter  II. 

Finally,  tracking  the  motion  of  the  lower  extremities  in  real  time  would  be  very 
important  as  well. 
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APPENDIX  A 


This  Appendix  contains  the  Matlab  program  which  tested  the  mathematical  model 
of  the  human-gait.  (Comments  are  highlighted  in  green.) 


%  LTJG  (M)  I.  PANTAZIS  (HN) 

%  09-01-2005 

%  FORWARD  KINEMATICS  TEST 

%  In  this  test  we  are  going  to  verify  that  the  background  theory  that  we 
%  introduced  and  the  formulas  that  we  have  developed  apply  for  random  joint  angles. 

clear 

clc 

alpha_0=0;,  a0=0;,  dl=0;,  theta_l=pi/3; 

11=0.45;  %  Note  that  the  lengths  are  in  meters 

alpha_l=0;,  al=ll;,  d2=0;,  theta_2=pi/6; 

T_01=[cos(theta_l),-sin(theta_l),0,0;sin(theta_l),cos(theta_l),0,0;0,0,l,0;0, 0,0,1]; 
T_12=[cos(theta_2),-sin(theta_2),  0,1  l;sin(theta_2),cos(theta_2),0,0;0,0,l,0;0, 0,0,1]; 
T_02=T_01*T_12; 

12=0.4; 

P2=[12;0;0;l]; 

P_02=T_02*P2 

%  Since  theta  l  plus  theta_2  is  equal  to  pi/2  and  since  we  consider  this 
%  model  to  be  two-dimensional,  the  projection  of  the  pelvis  axis  to  the 
%  x-axis  is  going  to  be  almost  parallel  to  the  ground.  Therefore 
%  theta_3=3*pi/2. 

theta_3=3*pi/2; 

T_23=[cos(theta_3),-sin(theta_3),0,12;sin(theta_3),cos(theta_3),0,0;0,0,l,0;0, 0,0,1]; 

T_03=T_02*T_23; 

l_pelvis=0.2; 

phi=pi/60; 

P3=[l_pelvis*sin(phi);l_pelvis*sin(-phi);0;l]; 

P_03=T_03*P3 

theta_4=5*pi/3; 

alpha_3=0; 

a3=l_pelvis*sin(phi); 

d4=0; 

T_34=[cos(theta_4),-sm(theta_4),0,l_pelvis*sin(phi);sin(theta_4),cos(theta_4),0,0;0, 0,1, 0;0, 0,0,1]; 
I  04  I  03* f  34; 

P4=[12;0;0;l]; 

P_04=T_04*P4 

theta_5=-1.5*pi/18; 

alpha_4=0; 

a4=12; 

d5=0; 

T_45=[cos(theta_5),-sin(theta_5),0,12;sin(theta_5),cos(theta_5),0,0;0,0,l,0;0, 0,0,1]; 
T_05=T_04*T_45; 

P5=[l  1  ;0;0;  1  ]; 

P  05=T  05*P5 
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APPENDIX  B 


This  Appendix  contains  the  Matlab  simulation  program  for  the  case  presented  in 
Chapter  IV.  (Comments  are  highlighted  in  green.) 

%  21  FEB  2005 

%  LTJG  (M)  PANTAZIS  IOANNIS  (HN) 

%  ANIMATION  -  FOUR  LIMPS  (NO  PELVIS)-  FULL  GAIT  CYCLE-CHANGEABLE  STEP- 
TREADMILL 

clear 

clc 

11=0.45; 

12=0.4; 

13=12; 

14=11; 

x_0=l; 

y_0=0; 

theta _ 1 _ initial=-(pi/6); 

theta_2 _ initial=-(pi/6); 

theta_3_initial=-(l  l*pi/12); 

x_5=x_0+ll*sin(theta_l_initial)+12*sin(theta_2_initial)+13*sin(theta_3_initial)-sqrt((llA2)- 

(((ll+12)*cos(theta_l_initial)+13*cos(theta_3_initial))A2)); 

x=[x_0,x_0+ll*sin(theta_l_initial),x_0+ll*sm(theta_l_initial)+12*sm(theta_2_initial),x_0+ll*sin 

(theta_l_initial)+12*sin(theta_2_mitial)+13*sm(theta_3_initial),x_5]; 

y=[y_0,ll*cos(theta_l_initial),(ll+12)*cos(theta_l_initial),(ll+12)*cos(theta_l_initial)+13*cos(the 

ta_3_initial),0]; 

nframes=50; 

h=plot(x,y,'m') 

set(h,'MarkerSize',  1 8); 

axis([-l  8  0  4]) 

axis  square 

grid 

kl=10; 

k2=20; 

k3=26; 

k4=30; 

k5=37; 

k6=45; 

for  i=l:4 

for  k  =  1  :nframes 

if  k<=kl 

k 

alpha=(  1 00-rand)/l  00 

theta_l_inc=alpha*pi/60; 

theta_2_inc=pi/ 60 ; 

theta_3_a_inc=pi/ 1 80; 

theta_4_a _inc=pi/ 150; 

theta  1  =theta_  l_initial+k*  theta_  1  _inc ; 

theta2=theta_2_initial 

theta3=(theta_3_initial-k*theta_3_a_inc) 
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theta4_constant=-(pi/2)-atan((0-((ll+12)*cos(theta_l_initial)+13*cos(theta_3_initial)))/(x_5- 

(x_0+ll*sin(theta_l_initial)+12*sin(theta_2_initial)+13*sin(theta_3_initial)))); 

theta4=theta4_constant+k*theta_4_a_inc; 

x_init=[x_0,x_0,x_0,x_0,x_0]; 

x(i+l,:)=x(i,l)+[0,ll*sin(thetal),ll*sin(thetal)+12*sin(theta2),ll*sin(thetal)+12*sin(theta2)+13*si 

n(theta3),ll*sin(thetal)+12*sin(theta2)+13*sin(theta3)+14*sin(theta4)] 

X=x(i+1,:) 

y=[y_0,ll  *cos(theta  1  ),1 1  *cos(theta  1  )+12  *cos(theta2),l  1  *cos(theta  1  )+12  *cos(theta2)+13  *cos(theta3 
),H*cos(thetal)+12*cos(theta2)+13*cos(theta3)+14*cos(theta4)] 
elseif  k<=k2 
k 

theta_3_b_inc=pi/60; 

theta_4_b_inc=pi/60; 

thetal=theta_l_initial+kl*theta_l_inc; 

theta2=theta_2_initial+(k-k  1 )  *  theta_2_inc ; 

theta3=theta_3_initial-kl*theta_3_a_inc-(k-kl)*theta_3_b_inc; 

theta4=theta4_constant+k  1  *theta_4_a_inc-(k-k  1  )*theta_4_b_inc; 

x(i+l,:)=x(i,l)+[0,ll*sin(thetal),ll*sin(thetal)+12*sin(theta2),ll*sin(thetal)+12*sin(theta2)+13*si 

n(theta3),ll*sin(thetal)+12*sin(theta2)+13*sin(theta3)+14*sin(theta4)] 

X=x(i+1,:) 

y=[y_0,ll  *cos(theta  1  ),1 1  *cos(theta  1  )+12  *cos(theta2),l  1  *cos(theta  1  )+12  *cos(theta2)+13  *cos(theta3 
),H*cos(thetal)+12*cos(theta2)+13*cos(theta3)+14*cos(theta4)] 
elseif  k<=k3 
k 

theta_3_c_inc=pi/80; 

theta_4_c_inc=pi/90; 

theta  1  =theta_  l_initial+(k-k  1 )  *  theta_  l_inc ; 
theta2=theta_2_initial+(k-k  1 )  *  theta_2_inc ; 

theta3=theta_3_initial-kl*theta_3_a_inc-(k2-kl)*theta_3_b_inc-(k-k2)*theta_3_c_inc; 
theta4=theta4_constant+kl*theta_4_a_inc-(k2-kl)*theta_4_b_inc-(k-k2)*theta_4_c_inc; 
x(i+l,:)=x(i,l)+[0,ll*sin(thetal),ll*sin(thetal)+12*sin(theta2),ll*sin(thetal)+12*sin(theta2)+13*si 
n(theta3),l  1  *sin(theta  1  )+12*sin(theta2)+13*sin(theta3  )+14*sin(theta4)] 

X=x(i+1,:) 

y=[y_0,ll*cos(thetal),ll*cos(thetal)+12*cos(theta2),ll*cos(thetal)+12*cos(theta2)+13*cos(theta3 
),H*cos(thetal)+12*cos(theta2)+13*cos(theta3)+14*cos(theta4)] 
elseif  k<=k4 
k 

theta_2_d_inc=pi/90; 

theta_3_d_inc=pi/145; 

theta_4_d_inc=pi/47; 

thetal=theta_l_initial+(k3-kl)*theta_l_inc+(k-k3)*theta_l_inc; 

theta2=theta_l_initial+(k3-kl)*theta_l_inc-(k-k3)*theta_2_d_inc; 

theta3=theta_3_initial-kl*theta_3_a_inc-(k2-kl)*theta_3_b_inc-(k3-k2)*theta_3_c_inc+(k- 

k3)*theta_3_d_inc; 

theta4=theta4_constant+kl*theta_4_a_mc-(k2-kl)*theta_4_b_inc-(k3-k2)*theta_4_c_inc-(k- 

k3)*theta_4_d_inc; 

x(i+l,:)=x(i,l)+[0,ll*sin(thetal),ll*sin(thetal)+12*sin(theta2)dl*sin(thetal)+12*sin(theta2)+13*si 

n(theta3)dl*sin(thetal)+12*sin(theta2)+13*sin(theta3)+14*sin(theta4)] 

X=x(i+1,:) 

y=[y_0,ll  *cos(theta  1  ),1 1  *cos(theta  1  )+12  *cos(theta2),l  1  *cos(theta  1  )+12  *cos(theta2)+13  *cos(theta3 
),H*cos(thetal)+12*cos(theta2)+13*cos(theta3)+14*cos(theta4)] 
elseif  k<=k5 
k 

theta_l_e_inc=pi/40; 

theta_2_e_inc=pi/90; 

theta_3_e_inc=pi/40; 


72 


theta_4_e_inc=pi/  8  0 ; 

thetal_f=theta_l_initial+(k3-kl)*theta_l_inc+(k4-k3)*theta_l_inc; 

theta2_f=theta_l_initial+(k3-kl)*theta_l_inc-(k4-k3)*theta_2_d_inc; 

theta3_f=theta_3_initial-kl*theta_3_a_inc-(k2-kl)*theta_3_b_inc-(k3-k2)*theta_3_c_inc+(k4- 

k3)*theta_3_d_inc; 

theta4_f=theta4_constant+k  1  *theta_4_a_inc-(k2-k  1  )*theta_4_b_inc-(k3  -k2  )*theta_4_c_inc-(k4- 
k3)*theta_4_d_inc; 

x_01=[0,ll*sin(thetal_f),ll*sin(thetal_f)+12*sin(theta2_f),ll*sin(thetal_f)+12*sin(theta2_f)+13*s 

in(theta3_f),ll*sin(thetal_f)+12*sin(theta2_f)+13*sin(theta3_f)+14*sin(theta4_f)] 

y_01=[y_0,ll*cos(thetal_f),ll*cos(thetal_f)+12*cos(theta2_f),ll*cos(thetal_f)+12*cos(theta2_f) 
+13*cos(theta3_f),ll*cos(thetal_f)+12*cos(theta2_f)+13*cos(theta3_f)+14*cos(theta4_f)] 
theta4=theta4_f+(k-k4)*theta_4_e_inc; 
theta3=theta3_f+(k-k4)*theta_3_e_inc; 
theta2=theta2_f-(k-k4)*theta_2_e_inc; 
theta  1  =theta  1  _f+(k-k4  )*  theta_  l_e_inc ; 

x(i+l,:)=x(i,l)+[x_01(l,5)-14*sin(theta4)-13*sin(theta3)-12*sin(theta2)-ll*sin(thetal),x_01(l,5)- 

14*sin(theta4)-13*sin(theta3)-12*sin(theta2),x_01(l,5)-14*sin(theta4)-13*sin(theta3),x_01(l,5)- 

14*sin(theta4),x_01(l,5)] 

X=x(i+1,:) 

y=[-14*cos(theta4)-13  *cos(theta3  )-12*cos(theta2)-l  1  *cos(thetal  ),-14*cos(theta4)-13  *cos(theta3  )- 
12*cos(theta2),-14*cos(theta4)-13*cos(theta3),-14*cos(theta4),0] 
elseif  k<=k6 
k 

theta_  l_g_inc=pi/3  0; 
theta_2_g_inc=pi/40; 
theta_3_g_inc=pi/80; 
theta_4_g_inc=pi/80; 

theta4=theta4_f+(k5  -k4 )  *  theta_4_e_inc+(k-k5 )  *  theta_4_g_inc; 
theta3=theta3_f+(k5-k4)*theta_3_e_inc+(k-k5)*theta_3_g_inc; 
theta2=theta2_f-(k5-k4)*theta_2_e_inc-(k-k5)*theta_2_g_inc; 
thetal=thetal_f+(k5-k4)*theta_l_e_inc-(k-k5)*theta_l_g_inc; 

x(i+l,:)=x(i,l)+[x_01(l,5)-14*sin(theta4)-13*sin(theta3)-12*sin(theta2)-ll*sin(thetal),x_01(l,5)- 

14*sin(theta4)-13*sin(theta3)-12*sin(theta2),x_01(l,5)-14*sin(theta4)-13*sin(theta3),x_01(l,5)- 

14*sin(theta4),x_01(l,5)] 

X=x(i+1,:) 

y=[-14*cos(theta4)-13*cos(theta3)-12*cos(theta2)-ll*cos(thetal),-14*cos(theta4)-13*cos(theta3)- 
12*cos(theta2),-14*cos(theta4)-13*cos(theta3),-14*cos(theta4),0] 
else  1<< — nt'i'amcs 
k 

theta_  l_h_inc=pi/22 ; 
theta_2_h_inc=pi/95; 
theta_3_h_inc=pi/1000; 
theta_4_h_inc=pi/1000; 

theta4=theta4_f+(k5-k4)*theta_4_e_inc+(k6-k5)*theta_4_g_mc+(k-k6)*theta_4_h_inc; 
theta3=theta3_f+(k5-k4)*theta_3_e_inc+(k6-k5)*theta_3_g_inc+(k-k6)*theta_3_h_inc; 
theta2=theta2_f-(k5-k4)*theta_2_e_inc-(k6-k5)*theta_2_g_inc+(k-k6)*theta_2_h_inc; 
theta  I=thetal_f+(k5-k4)*theta_l_e_inc-(k6-k5)*theta_l_g_inc-(k-k6)*theta_l_h_inc; 
x(i+l,:)=x(i,l)+[x_01(l,5)-14*sin(theta4)-13*sin(theta3)-12*sin(theta2)-ll*sin(thetal),x_01(l,5)- 
14*sin(theta4)-13*sin(theta3)-12*sin(theta2),x_01(l,5)-14*sin(theta4)-13*sin(theta3),x_01(l,5)- 
14*sin(theta4),x_01(l,5)] 

X=x(i+1,:) 

y=[-14*cos(theta4)-13*cos(theta3)-12*cos(theta2)-ll*cos(thetal),-14*cos(theta4)-13*cos(theta3)- 

12*cos(theta2),-14*cos(theta4)-13*cos(theta3),-14*cos(theta4),0] 

end 

set(h,'XData',X,'YData',y) 

M(k,i)  =  getframe; 
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end 

end 

total_distance=X(  1 , 1  )-x_5 
distance_per_cycle=total_distance/i 
%  movie(M,3) 
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APPENDIX  C 


This  Appendix  contains  the  Matlab  simulation  program  with  the  enhancements 
presented  in  Chapter  V.  (Comments  are  highlighted  in  green.) 


%  22  APR  2005 
%  PANTAZIS  IOANNIS 

%  ANIMATION  -  RIGHT  FOOT  (WITH  MASS),  SHANK,  THIGH,  PELVIS  &  LEFT  THIGH, 
SHANK  AND 

%  FOOT  (MASS)  -  FULL  GAIT  CYCLE 

clear 

clc 

lo=0. 1 ; 

11=0.22; 

12=0.45; 

13=0.4; 

14=13; 

15=12; 

16=11; 

loo=lo; 

l_pel=0.4; 

x=[l  -lo*sin(pi/6),  1, 1+1 1  *cos(pi/6),  l-lo*sin(pi/6),  1  -lo*sin(pi/6)-12*sin(pi/8),  1  -lo*sin(pi/6)- 
12*sin(pi/8)-13*sin(pi/8),l-lo*sin(pi/6)-12*sin(pi/8)-13*sin(pi/8)-l_pel*tan(pi/45),l-lo*sin(pi/6)- 
12*sin(pi/8)-13*sin(pi/8)-l_pel*tan(pi/45)-14*sin(pi/10),l-lo*sin(pi/6)-12*sin(pi/8)-13*sin(pi/8)- 
l_pel*tan(pi/45)-14*sin(pi/10)-15*sin(pi/4.25),l-lo*sin(pi/6)-12*sin(pi/8)-13*sin(pi/8)-l_pel*tan(pi/45)- 
14*sin(pi/10)-15*sin(pi/4.25)-loo*sin(pi/4),l-lo*sin(pi/6)-12*sin(pi/8)-13*sin(pi/8)-l_pel*tan(pi/45)- 
14*sin(pi/10)-15*sin(pi/4.25)-loo*sin(pi/4)+16*cos(pi/6),l-lo*sin(pi/6)-12*sin(pi/8)-13*sin(pi/8)- 
l_pel*tan(pi/45)-14*sin(pi/10)-15*sin(pi/4.25)]; 

y=[lo*cos(pi/6),0,ll*sin(pi/6),lo*cos(pi/6),lo*cos(pi/6)+12*cos(pi/8),lo*cos(pi/6)+12*cos(pi/8)+13 

*cos(pi/8),lo*cos(pi/6)+12*cos(pi/8)+13*cos(pi/8)+l_pel*tan(pi/60),lo*cos(pi/6)+12*cos(pi/8)+13*cos(pi/8) 

+l_pel*tan(pi/60)-14*cos(pi/10),lo*cos(pi/6)+12*cos(pi/8)+13*cos(pi/8)+l_pel*tan(pi/60)-14*cos(pi/10)- 

15*cos(pi/4.25),lo*cos(pi/6)+12*cos(pi/8)+13*cos(pi/8)+l_pel*tan(pi/60)-14*cos(pi/10)-15*cos(pi/4.25)- 

loo*cos(pi/4),lo*cos(pi/6)+12*cos(pi/8)+13*cos(pi/8)+l_pel*tan(pi/60)-14*cos(pi/10)-15*cos(pi/4.25)- 

loo*cos(pi/4)-16*sin(pi/6),lo*cos(pi/6)+12*cos(pi/8)+13*cos(pi/8)+l_pel*tan(pi/60)-14*cos(pi/10)- 

15*cos(pi/4.25)]; 

nframes=480; 
theta_0_inc=-3*pi/1800; 
theta_l_inc=-3  *pi/ 1 800; 
theta_2_a_inc=-pi/900; 
theta_2_b_mc=-pi/5000; 
theta_2_c_inc=-pi/ 1 000; 
theta_2_d_inc=-pi/ 500; 
theta_3_b_inc=-pi/800; 
theta_3_d_inc=p  i/850; 
theta_4_a_inc=-pi/ 1 500; 
theta_4_b_mc=-pi/400; 
theta_4_c_inc=p  i/2000; 
theta_5_a_inc=p  i/2000; 
theta_5_b_inc=-pi/l  000; 
theta_5_c_inc=-pi/3  00 ; 
theta_5_d_mc=-pi/1000; 
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theta_6_a_inc=pi/ 1 000; 

theta_pel_hor_a_inc=pi/9000; 

theta_pel_ver_a_inc=p  i/12000; 

theta_pel_hor_b_inc=pi/4500; 

theta_pel_ver_b_inc=pi/6000; 

h=plot(x,y) 

set(h,'MarkerSize',5); 

axis([0  3  -0.2  2.8]) 

grid 

thetal=pi/6; 
theta2=pi/8; 
theta3=pi/8; 
theta4=pi/10; 
theta5=p  i/4.2; 
theta6=pi/6; 
theta_pel_hor=pi/45; 
theta_pel_ver=pi/60; 
for  k  =  1  :nframes 
if  k<=100 
k; 

theta  1  =theta  1  +theta_  l_inc ; 

theta2=theta2+theta_2_a_inc; 

theta4=theta4+theta_4_a_inc; 

theta5=theta5+theta_5_a_inc; 

theta6=theta6+theta_6_a_inc; 

theta_pel_hor=theta_pel_hor-theta_pel_hor_a_inc; 

theta_pel_ver=theta_pel_ver-theta_pel_ver_a_inc; 

x=[l-lo*sin(thetal),l,l+ll*cos(thetal),l-lo*sin(thetal),l-lo*sin(thetal)-12*sin(theta2),l- 

lo*sin(thetal)-12*sin(theta2)-13*sin(theta3),l-lo*sin(thetal)-12*sin(theta2)-13*sin(theta3)- 

l_pel*tan(theta_pel_hor),l-lo*sin(thetal)-12*sin(theta2)-13*sin(theta3)-l_pel*tan(theta_pel_hor)- 

14*sin(theta4),l-lo*sin(thetal)-12*sin(theta2)-13*sin(theta3)-l_pel*tan(theta_pel_hor)-14*sin(theta4)- 

15*sin(theta5),l-lo*sin(thetal)-12*sin(theta2)-13*sin(theta3)-l_pel*tan(theta_pel_hor)-14*sin(theta4)- 

15*sin(theta5)-loo*sin(theta6),l-lo*sin(thetal)-12*sin(theta2)-13*sin(theta3)-l_pel*tan(theta_pel_hor)- 

14*sin(theta4)-15*sin(theta5)-loo*sin(theta6)+16*cos(theta6),l-lo*sin(thetal)-12*sin(theta2)-13*sin(theta3)- 

l_pel*tan(theta_pel_hor)-14*sin(theta4)-15*sin(theta5)]; 

y=[lo*cos(thetal),0,ll*sin(thetal),lo*cos(thetal),lo*cos(thetal)+12*cos(theta2),lo*cos(thetal)+12 
*cos(theta2)+13*cos(theta3),lo*cos(thetal)+12*cos(theta2)+13*cos(theta3)+l_pel*tan(theta_pel_ver),lo*cos 
(thetal  )+12*cos(theta2)+13  *cos(theta3  )+l_pel*tan(theta_pel_ver)- 

14*cos(theta4),lo*cos(thetal)+12*cos(theta2)+13*cos(theta3)+l_pel*tan(theta_pel_ver)-14*cos(theta4)- 

15*cos(theta5),lo*cos(thetal)+12*cos(theta2)+13*cos(theta3)+l_pel*tan(theta_pel_ver)-14*cos(theta4)- 

15*cos(theta5)-loo*cos(theta6),lo*cos(thetal)+12*cos(theta2)+13*cos(theta3)+l_pel*tan(theta_pel_ver)- 

14*cos(theta4)-15*cos(theta5)-loo*cos(theta6)- 

16*sin(theta6),lo*cos(thetal)+12*cos(theta2)+13*cos(theta3)+l_pel*tan(theta_pel_ver)-14*cos(theta4)- 

15*cos(theta5)]; 

elseif  k<=200 

k; 

theta2=theta2+theta_2_b_inc; 
theta3=theta3+theta_3_b_inc; 
theta4=theta4+theta_4_a_inc; 
theta5=theta5+theta_5_b_inc; 
theta6=theta6- 1 .5*theta_6_a_inc; 
theta_pel_hor=theta_pel_hor-theta_pel_hor_a_inc; 
theta_pel_ver=theta_pel_ver-theta_pel_ver_a_inc; 

x=[l-lo*sin(thetal),l,l+ll*cos(thetal),l-lo*sin(thetal),l-lo*sin(thetal)-12*sin(theta2),l- 

lo*sin(thetal)-12*sin(theta2)-13*sin(theta3),l-lo*sin(thetal)-12*sin(theta2)-13*sin(theta3)- 

l_pel*tan(theta_pel_hor),l-lo*sin(thetal)-12*sin(theta2)-13*sin(theta3)-l_pel*tan(theta_pel_hor)- 
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14*sin(theta4),l-lo*sin(thetal)-12*sin(theta2)-13*sin(theta3)-l_pel*tan(theta_pel_hor)-14*sin(theta4)- 

15*sin(theta5),l-lo*sin(thetal)-12*sin(theta2)-13*sin(theta3)-l_pel*tan(theta_pel_hor)-14*sin(theta4)- 

15*sin(theta5)-loo*sin(theta6),l-lo*sin(thetal)-12*sin(theta2)-13*sin(theta3)-l_pel*tan(theta_pel_hor)- 

14*sin(theta4)-15*sin(theta5)-loo*sin(theta6)+16*cos(theta6),l-lo*sin(thetal)-12*sin(theta2)-13*sin(theta3)- 

l_pel*tan(theta_pel_hor)-14*sin(theta4)-15*sin(theta5)]; 

y=[lo*cos(thetal),0,ll*sin(thetal),lo*cos(thetal),lo*cos(thetal)+12*cos(theta2),lo*cos(thetal)+12*cos(thet 

a2)+13*cos(theta3),lo*cos(thetal)+12*cos(theta2)+13*cos(theta3)+l_pel*tan(theta_pel_ver),lo*cos(thetal)+ 

12*cos(theta2)+13*cos(theta3)+l_pel*tan(theta_pel_ver)- 

14*cos(theta4),lo*cos(thetal)+12*cos(theta2)+13*cos(theta3)+l_pel*tan(theta_pel_ver)-14*cos(theta4)- 

15*cos(theta5),lo*cos(thetal)+12*cos(theta2)+13*cos(theta3)+l_pel*tan(theta_pel_ver)-14*cos(theta4)- 

15*cos(theta5)-loo*cos(theta6),lo*cos(thetal)+12*cos(theta2)+13*cos(theta3)+l_pel*tan(theta_pel_ver)- 

14*cos(theta4)-15*cos(theta5)-loo*cos(theta6)- 

16*sin(theta6),lo*cos(thetal)+12*cos(theta2)+13*cos(theta3)+l_pel*tan(theta_pel_ver)-14*cos(theta4)- 

15*cos(theta5)]; 

elseif  k<=250 

k; 

theta  1  =theta  1  +theta_  l_inc; 

theta2=theta2+theta_2_c_inc; 

theta3  =theta3  +theta_2_c_inc; 

theta4=theta4+ 1 .2  *theta_4_b_inc; 

theta5=theta5+ 1 .3  *theta_5_c_inc; 

theta6=theta6-2*theta_6_a_inc; 

theta_pel_hor=theta_pel_hor-theta_pel_hor_b_inc; 

theta_pel_ver=theta_pel_ver-theta_pel_ver_b_inc; 

x=[l+ll-ll*cos(thetal)-lo*sin(thetal),l+ll-ll*cos(thetal),l+ll,l+ll-ll*cos(thetal)- 
lo*sin(thetal),l+ll-ll*cos(thetal)-lo*sin(thetal)-12*sin(theta2),l+ll-ll*cos(thetal)-lo*sin(thetal)- 
12*sin(theta2)-13*sin(theta3),l+H-ll*cos(thetal)-lo*sin(thetal)-12*sin(theta2)-13*sin(theta3)- 
l_pel*tan(theta_pel_hor),  1+1 1-11  *cos(thetal)-lo*sin(theta  1  )-12*sin(theta2)-13  *sin(theta3)- 
l_pel*tan(theta_pel_hor)-14*sin(theta4),  1+1 1  -1 1  *cos(thetal  )-lo*sin(  theta  1)-I2*sin(theta2)-13  *sin(theta3  )- 
l_pel*tan(theta_pel_hor)-14*sin(theta4)-15*sin(theta5),l+H-H*cos(thetal)-lo*sin(thetal)-12*sin(theta2)- 
13*sin(theta3)-l_pel*tan(theta_pel_hor)-14*sin(theta4)-15*sin(theta5)-loo*sin(theta6),l+ll-ll*cos(thetal)- 
lo*sin(thetal)-12*sin(theta2)-13*sin(theta3)-l_pel*tan(theta_pel_hor)-14*sin(theta4)-15*sin(theta5)- 
loo*sin(theta6)+16*cos(theta6),  1+1 1-11  *cos(thetal)-lo*sin(thetal  )-12*sin(theta2)-13*sin(theta3)- 
l_pel*tan(theta_pel_hor)-14*sin(theta4)-15*sin(theta5)]; 

y=[-ll*sin(thetal)+lo*eos(thetal),-ll*sin(thetal),0,-ll*sin(thetal)+lo*cos(thetal),- 
ll*sin(thetal)+lo*cos(thetal)+12*cos(theta2),- 
1 1  *sin(theta  1  )+lo*cos(theta  1  )+12*cos(theta2)+13*cos(theta3),- 

ll*sin(thetal)+lo*cos(thetal)+12*cos(theta2)+13*cos(theta3)+l_pel*tan(theta_pel_ver),- 
ll*sin(thetal)+lo*cos(thetal)+12*cos(theta2)+13*cos(theta3)+l_pel*tan(theta_pel_ver)-14*cos(theta4),- 
1 1  *sin(theta  1  )+lo*cos(theta  1  )+12  *cos(theta2  )+13  *cos(theta3  )+l_pel*tan(theta_pel_ver)-14*cos(theta4)- 
15  *cos(theta5),-l  1  *sin(theta  1  )+lo*cos(theta  1  )+12  *cos(theta2)+13  *cos(theta3  )+l_pel*tan(theta_pel_ver)- 
14*cos(theta4)-15*cos(theta5)-loo*cos(theta6),- 

ll*sin(thetal)+lo*cos(thetal)+12*cos(theta2)+13*cos(theta3)+l_pel*tan(theta_pel_ver)-14*cos(theta4)- 

15*cos(theta5)-loo*cos(theta6)-16*sin(theta6),- 

ll*sin(thetal)+lo*cos(thetal)+12*cos(theta2)+13*cos(theta3)+l_pel*tan(theta_pel_ver)-14*cos(theta4)- 

15*cos(theta5)]; 

elseif  k<=300 
k 

theta  1  =theta  1  +theta_  l_inc; 

theta2=theta2+ 1 . 6  7  *  theta_2_d_inc ; 

theta3=theta3-theta_3_d_inc; 

theta4=theta4+3  *theta_4_c_inc; 

theta5=theta5+ 1 .43  *theta_5_d_inc; 

theta6=theta6-3*theta_6_a_inc; 

theta_pel_hor=theta_pel_hor-theta_pel_hor_b_inc 
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theta_pel_ver=theta_pel_ver-theta_pel_ver_b_inc 

x=[l+ll-ll*cos(thetal)-lo*sin(thetal),l+ll-ll*cos(thetal),l+ll,l+ll-ll*cos(thetal)- 

lo*sin(thetal),l+ll-ll*cos(thetal)-lo*sin(thetal)-12*sin(theta2),l+ll-ll*cos(thetal)-lo*sin(thetal)- 
12*sin(theta2)-13  *sin(theta3),  1+1 1-11  *cos(theta  l)-lo*sin(theta  1  )-12*sin(theta2)-13  *sin(theta3)- 
l_pel*tan(theta_pel_hor),  1  +11-11  *cos(theta  1  )-lo*sin(theta  1  )-12  *sin(theta2)-13  *sin(theta3 )- 
l_pel*tan(theta_pel_hor)-14*sin(theta4),  1+1 1  -1 1  *cos(thetal  )-lo*sin(  theta  1)-I2*sin(theta2)-13  *sin(theta3  )- 
l_pel*tan(theta_pel_hor)-14*sin(theta4)-15*sin(theta5),l+ll-H*cos(thetal)-lo*sin(thetal)-12*sin(theta2)- 
13*sin(theta3)-l_pel*tan(theta_pel_hor)-14*sin(theta4)-15*sin(theta5)-loo*sin(theta6),l+ll-ll*cos(thetal)- 
lo*sin(thetal)-12*sin(theta2)-13*sin(theta3)-l_pel*tan(theta_pel_hor)-14*sin(theta4)-15*sin(theta5)- 
loo*sin(theta6)+16*cos(theta6),  1+1 1-11  *cos(thetal)-lo*sin(theta  1)-I2*sin(theta2)-I3*sin(theta3)- 
l_pel*tan(theta_pel_hor)-14*sin(theta4)-15*sin(theta5)] 

y=[-ll*sin(thetal)+lo*cos(thetal),-ll*sin(thetal),0,-ll*sin(thetal)+lo*cos(thetal),- 
ll*sin(thetal)+lo*cos(thetal)+12*cos(theta2),- 
1 1  *sin(theta  1  )+lo*cos(theta  1  )+12  *cos(theta2)+13  *cos(theta3  ),- 

ll*sin(thetal)+lo*cos(thetal)+12*cos(theta2)+13*cos(theta3)+l_pel*tan(theta_pel_ver),- 
H*sin(thetal)+lo*cos(thetal)+12*cos(theta2)+13*cos(theta3)+l_pel*tan(theta_pel_ver)-14*cos(theta4),- 
H*sin(thetal)+lo*cos(thetal)+12*cos(theta2)+13*cos(theta3)+l_pel*tan(theta_pel_ver)-14*cos(theta4)- 
15*cos(theta5),-l  1  *sin(theta  1  )+lo*cos(thetal  )+12*cos(theta2)+13  *cos(theta3  )+l_pel*tan(theta_pel_ver)- 
14*cos(theta4)-15*cos(theta5)-loo*cos(theta6),- 

H*sin(thetal)+lo*cos(thetal)+12*cos(theta2)+13*cos(theta3)+l_pel*tan(theta_pel_ver)-14*cos(theta4)- 

15*cos(theta5)-loo*cos(theta6)-16*sin(theta6),- 

H*sin(thetal)+lo*cos(thetal)+12*cos(theta2)+13*cos(theta3)+l_pel*tan(theta_pel_ver)-14*cos(theta4)- 

15*cos(theta5)] 

elseif  k<=360 
k 

theta  1  _swing=theta  1 ; 
theta2_swing=theta2 ; 
theta3_swing=theta3 ; 
theta4_swing=theta4 ; 
theta5_swing=theta5 ; 
theta6_swing=theta6 
theta_pel_hor_swing=theta_pel_hor; 
theta_pel_ver_swing=theta_pel_ver; 
theta6_e_inc=pi/ 450; 
theta5_e_inc=p  i/800; 
theta4_e_inc=p  i/700; 
theta3_e_inc=p  i/450; 
theta2_e_inc=-pi/7 5  0 ; 
theta  1  _e_inc=-pi/7 5  0 ; 
theta_pel_hor_e_inc=p  i/2700; 
theta_pel_ver_e_inc=pi/3600; 
theta_6=theta6_swing+(k-300)*theta6_e_inc; 
theta_5=theta5_swing+(k-300)*theta5_e_inc; 
theta_4=theta4_swing+(k-3  00 )  *  theta4_e_inc ; 
theta_3=theta3_swing+(k-300)*theta3_e_inc; 
theta_2=theta2_swing+(k-300)*theta2_e_inc; 
theta_  1  =theta  1  _swing+(k-3  00 )  *  theta  l_e_inc ; 
theta_pel_hor_2=theta_pel_hor_swing+(k-300)*theta_pel_hor_e_inc; 
theta_pel_ver_2=theta_pel_ver_swing+(k-300)*theta_pel_ver_e_inc; 
x_s=l+ll-ll*cos(thetal_swing)-lo*sin(thetal_swing)-12*sin(theta2_swing)- 
13*sin(theta3_swing)-l_pel*tan(theta_pel_hor_swing)-14*sin(theta4_swing)-15*sin(theta5_swing)- 
loo*sin(theta5_swing) 
y_s=0; 

x=[x_s+lo*sin(theta_6),x_s+16*cos(theta_6),x_s,x_s+lo*sin(theta_6),x_s+lo*sin(theta_6)+15*sin(theta_5), 

x_s+lo*sin(theta_6)+15*sin(theta_5)+14*sin(theta_4),x_s+lo*sin(theta_6)+15*sin(theta_5)+14*sin(theta_4) 
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+l_pel*tan(theta_pel_hor_2),x_s+lo*sin(theta_6)+15*sin(theta_5)+14*sin(theta_4)+l_pel*tan(theta_pel_hor 

_2)+13*sin(theta_3),x_s+lo*sin(theta_6)+15*sin(theta_5)+14*sin(theta_4)+l_pel*tan(theta_pel_hor_2)+13*s 

in(theta_3)+12*sin(theta_2),x_s+lo*sin(theta_6)+15*sin(theta_5)+14*sin(theta_4)+l_pel*tan(theta_pel_hor_ 

2)+13*sin(theta_3)+12*sin(theta_2)+lo*sin(theta_l),x_s+lo*sin(theta_6)+15*sin(theta_5)+14*sin(theta_4)+l 

_pel*tan(theta_pel_hor_2)+13*sin(theta_3)+12*sin(theta_2)+lo*sin(theta_l)+ll*cos(theta_l),x_s+lo*sin(th 

eta_6)+15*sin(theta_5)+14*sin(theta_4)+l_pel*tan(theta_pel_hor_2)+13*sin(theta_3)+12*sin(theta_2)] 

y=[loo*cos(theta_6),- 

16*sin(theta_6),0,loo*cos(theta_6),lo*cos(theta_6)+15*cos(theta_5),loo*cos(theta_6)+15*cos(theta_5)+14*c 

os(theta_4),loo*cos(theta_6)+15*cos(theta_5)+14*cos(theta_4)- 

l_pel*tan(theta_pel_ver_2),loo*cos(theta_6)+15*cos(theta_5)+14*cos(theta_4)-l_pel*tan(theta_pel_ver_2)- 

13*cos(theta_3),loo*cos(theta_6)+15*cos(theta_5)+14*cos(theta_4)-l_pel*tan(theta_pel_ver_2)- 

13*cos(theta_3)-12*cos(theta_2),loo*cos(theta_6)+15*cos(theta_5)+14*cos(theta_4)- 

l_pel*tan(theta_pel_ver_2)-13*cos(theta_3)-12*cos(theta_2)- 

lo*cos(theta_l),loo*cos(theta_6)+15*cos(theta_5)+14*cos(theta_4)-l_pel*tan(theta_pel_ver_2)- 

13*cos(theta_3)-12*cos(theta_2)- 

lo*cos(theta_l)+ll*sin(theta_l),loo*cos(theta_6)+15*cos(theta_5)+14*cos(theta_4)- 
l_pel*tan(theta_pel_ver_2)-13*cos(theta_3)-12*cos(theta_2)] 
elseif  k<=420 
k 

theta5_f_inc=pi/ 1000; 
theta4_f_inc=pi/ 1000; 
theta3_f_inc=p  i/300; 
theta2_f_inc=pi/ 1 60; 
theta  1  _f_inc=pi/23  0; 
theta_pel_hor_f_inc=pi/2700; 
theta_pel_ver_f_inc=pi/3600; 
theta_6=theta6_swing+60*theta6_e_inc; 
theta_5=theta5_swing+60*theta5_e_inc+(k-360)*theta5_f_inc; 
theta_4=theta4_swing+60*theta4_e_inc+(k-360)*theta4_f_inc 
theta_3=theta3_swing+60*theta3_e_inc+(k-360)*theta3_f_inc; 
theta_2=theta2_swing+60*theta2_e_inc+(k-360)*theta2_f_inc; 
theta_l=thetal_swing+60*thetal_e_inc+(k-360)*thetal_f_inc; 
theta_pel_hor_2=theta_pel_hor_swing+60*theta_pel_hor_e_inc+(k- 
360)*theta_pel_hor_f_inc; 

theta_pel_ver_2=theta_pel_ver_swing+60*theta_pel_ver_e_inc+(k- 

360)*theta_pel_ver_f_inc; 

x=[x_s+lo*sin(theta_6),x_s+16*cos(theta_6),x_s,x_s+lo*sin(theta_6),x_s+lo*sin(theta_6)+15*sin(theta_5), 

x_s+lo*sin(theta_6)+15*sin(theta_5)+14*sin(theta_4),x_s+lo*sin(theta_6)+15*sin(theta_5)+14*sin(theta_4) 

+l_pel*tan(theta_pel_hor_2),x_s+lo*sin(theta_6)+15*sin(theta_5)+14*sin(theta_4)+l_pel*tan(theta_pel_hor 

_2)+13*sin(theta_3),x_s+lo*sin(theta_6)+15*sin(theta_5)+14*sin(theta_4)+l_pel*tan(theta_pel_hor_2)+13*s 

in(theta_3)+12*sin(theta_2),x_s+lo*sin(theta_6)+15*sin(theta_5)+14*sin(theta_4)+l_pel*tan(theta_pel_hor_ 

2)+13*sin(theta_3)+12*sin(theta_2)+lo*sin(theta_l),x_s+lo*sin(theta_6)+15*sin(theta_5)+14*sin(theta_4)+l 

_pel*tan(theta_pel_hor_2)+13*sin(theta_3)+12*sin(theta_2)+lo*sin(theta_l)+ll*cos(theta_l),x_s+lo*sin(th 

eta_6)+15*sin(theta_5)+14*sin(theta_4)+l_pel*tan(theta_pel_hor_2)+13*sin(theta_3)+12*sin(theta_2)] 

y=[loo*cos(theta_6),- 

16*sin(theta_6),0,loo*cos(theta_6),lo*cos(theta_6)+15*cos(theta_5),loo*cos(theta_6)+15*cos(theta_5)+14*c 

os(theta_4),loo*cos(theta_6)+15*cos(theta_5)+14*cos(theta_4)- 

l_pel*tan(theta_pel_ver_2),loo*cos(theta_6)+15*cos(theta_5)+14*cos(theta_4)-l_pel*tan(theta_pel_ver_2)- 

13*cos(theta_3),loo*cos(theta_6)+15*cos(theta_5)+14*cos(theta_4)-l_pel*tan(theta_pel_ver_2)- 

13*cos(theta_3)-12*cos(theta_2),loo*cos(theta_6)+15*cos(theta_5)+14*cos(theta_4)- 

l_pel*tan(theta_pel_ver_2)-13*cos(theta_3)-12*cos(theta_2)- 

lo*cos(theta_l),loo*cos(theta_6)+15*cos(theta_5)+14*cos(theta_4)-l_pel*tan(theta_pel_ver_2)- 

13*cos(theta_3)-12*cos(theta_2)- 

lo*cos(theta_l)+ll*sin(theta_l),loo*cos(theta_6)+15*cos(theta_5)+14*cos(theta_4)- 

l_pel*tan(theta_pel_ver_2)-13*cos(theta_3)-12*cos(theta_2)] 
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else  k<=480 
k 

theta6_g_inc=-pi/350; 
theta5_g_inc=pi/303; 
theta4_g_inc=pi/ 1200; 
theta3_g_inc=-pi/500; 
theta2_g_inc=pi/ 1800; 
theta  1  _g_inc=pi/ 570; 
theta_pel_hor_g_inc=p  i/2700; 
theta_pel_ver_g_inc=pi/3600; 

theta_6=theta6_swing+60*theta6_e_inc+(k-420)*theta6_g_inc; 

theta_5=theta5_swing+60*theta5_e_inc+60*theta5_f_inc+(k-420)*theta5_g_inc; 

theta_4=theta4_swing+60*theta4_e_inc+60*theta4_f_inc+(k-420)*theta4_g_inc 

theta_3=theta3_swing+60*theta3_e_inc+60*theta3_f_inc+(k-420)*theta3_g_inc; 

theta_2=theta2_swing+60*theta2_e_inc+60*theta2_f_inc+(k-420)*theta2_g_inc; 

theta_l=thetal_swing+60*thetal_e_inc+60*thetal_f_inc+(k-420)*thetal_g_inc; 

theta_pel_hor_2=theta_pel_hor_swing+60*theta_pel_hor_e_inc+60*theta_pel_hor_f_inc; 

theta_pel_ver_2=theta_pel_ver_swing+60*theta_pel_ver_e_inc+60*theta_pel_ver_f_inc; 

x=[x_s+16-lo*sin(theta_6)-16*cos(theta_6),x_s+16,x_s+16-16*cos(theta_6),x_s+16- 

16*cos(theta_6)-lo*sin(theta_6),x_s+16-16*cos(theta_6)+lo*sin(theta_6)+15*sin(theta_5),x_s+16- 

16*cos(theta_6)+lo*sin(theta_6)+15*sin(theta_5)+14*sin(theta_4),x_s+16- 

16*cos(theta_6)+lo*sin(theta_6)+15*sin(theta_5)+14*sin(theta_4)+l_pel*tan(theta_pel_hor_2),x_s+16- 

16*cos(theta_6)+lo*sin(theta_6)+15*sin(theta_5)+14*sin(theta_4)+l_pel*tan(theta_pel_hor_2)+13*sin(theta 

_3),x_s+16- 

16*cos(theta_6)+lo*sin(theta_6)+15*sin(theta_5)+14*sin(theta_4)+l_pel*tan(theta_pel_hor_2)+13*sin(theta 

_3)+12*sin(theta_2),x_s+16- 

16*cos(theta_6)+lo*sin(theta_6)+15*sin(theta_5)+14*sin(theta_4)+l_pel*tan(theta_pel_hor_2)+13*sin(theta 

_3)+12*sin(theta_2)+lo*sin(theta_l),x_s+16- 

16*cos(theta_6)+lo*sin(theta_6)+15*sin(theta_5)+14*sin(theta_4)+l_pel*tan(theta_pel_hor_2)+13*sin(theta 

_3)+12*sin(theta_2)+lo*sin(theta_l)+ll*cos(theta_l),x_s+16- 

16*cos(theta_6)+lo*sin(theta_6)+15*sin(theta_5)+14*sin(theta_4)+l_pel*tan(theta_pel_hor_2)+13*sin(theta 

_3)+12*sin(theta_2)] 

y=[lo*cos(theta_6)-16*sin(theta_6),0,-16*sin(theta_6),lo*cos(theta_6)- 

16*sin(theta_6),lo*cos(theta_6)-16*sin(theta_6)+15*cos(theta_5),lo*cos(theta_6)- 

16*sin(theta_6)+15*cos(theta_5)+14*cos(theta_4),lo*cos(theta_6)- 

16*sin(theta_6)+15*cos(theta_5)+14*cos(theta_4)-l_pel*tan(theta_pel_ver_2),lo*cos(theta_6)- 

16*sin(theta_6)+15*cos(theta_5)+14*cos(theta_4)-l_pel*tan(theta_pel_ver_2)- 

13*cos(theta_3),lo*cos(theta_6)-16*sin(theta_6)+15*cos(theta_5)+14*cos(theta_4)- 

l_pel*tan(theta_pel_ver_2)-13*cos(theta_3)-12*cos(theta_2),lo*cos(theta_6)- 

16*sin(theta_6)+15*cos(theta_5)+14*cos(theta_4)-l_pel*tan(theta_pel_ver_2)-13*cos(theta_3)- 

12*cos(theta_2)-loo*cos(theta_l),lo*cos(theta_6)-16*sin(theta_6)+15*cos(theta_5)+14*cos(theta_4)- 

l_pel*tan(theta_pel_ver_2)-13*cos(theta_3)-12*cos(theta_2)- 

lo*cos(theta_l)+ll*sin(theta_l),lo*cos(theta_6)-16*sin(theta_6)+15*cos(theta_5)+14*cos(theta_4)- 

l_pel*tan(theta_pel_ver_2)-13*cos(theta_3)-12*cos(theta_2)] 

end 

set(h,'XData',x,'YData',y) 

M(k)  =  getframe; 
end 
end 

%  movie(M) 
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APPENDIX  D 


This  Appendix  contains  the  LISP  program  by  Professor  Robert  McGhee  that  was 
discussed  in  Chapter  V.  (Comments  are  highlighted  in  green.) 

;C:\\Documents  and  Settings\\mcghee\\My  DocumentsWMARG  SimulationWsagittal-foot- 
kinematics.cl" 

;This  code  was  written  in  Allegro  ANSI  Common  Lisp,  Version  6.2,  by  Prof. 

Robert  B.  McGhee  (mcghee@montereybay.com)  at  the  Naval  Postgraduate  School  in  Monterey, 
;CA.  Date  of  last  revision:  25  April,  2005. 

;x  is  forward  in  plane  of  progression,  y  is  up. 

(load  "C:/documents  and  settings/yun/desktop/nps  aca- 
demic/02_2005/moves_4472/final_project/n-link-iterative-dynamics.cl") 

(defclass  biped-foot  (planar-link) 

((heel-to-ankle-length  :accessor  heel-to-ankle-length  :initform  12)  ;A11  lengths  in  cm. 
(sole-length  :accessor  sole-length  :initform  24)  ;Measured  from  back  of  heel  to  start  of  big  toe. 
(sole-to-ankle-angle  :accessor  sole-to-ankle-angle  :initform  (/  pi  3))  ;Positive  counterclockwise, 
(sole-angle  :accessor  sole-angle)  ;Foot  flat  is  0  radians.  Toe  up  is  positive. 

(stride-length  :accessor  stride-length)  ;From  heel  strike  to  next  heel  strike  on  same  foot, 
(cycle-time  :accessor  cycle-time)  ;For  one  hill  stride. 

(support-state  :accessor  support-state)  ;Values  are:  swing,  heel,  sole,  toe. 

(heel-position  :accessor  heel-position) ;  '(x  y) 

(toe-position  :  accessor  toe-position) 

(ankle-position  :accessor  ankle-position) 

(waypoint-list  :accessor  waypoint-list) 

(time-stamp  : accessor  time-stamp  :initform  0))) 

(defclass  waypoint  (biped-foot) 

((waypoint-name  :accessor  waypoint-name) 

(waypoint-duration  :accessor  waypoint-duration)  formalized  to  one  second  cycle  time, 
(rotation-reference-point  :accessor  rotation-reference-point)))  ;To  next  waypoint. 

(defmethod  make-waypoint-list  ((foot  biped-foot)  number-of-waypoints) 

(do*  ((list  nil  (cons  (make-instance  'waypoint)  list)) 

(count  0  (1+  count))) 

((=  count  number-of-waypoints)  (setf  (waypoint-list  foot)  list)))) 

(defmethod  initialize-waypoint  ((foot  biped-foot)  index  name  duration  support  reference 
sole  heel  toe  ankle) 

(let*  ((list  (waypoint-list  foot))  (waypoint  (elt  list  index))  (wp  waypoint)) 

(setf  (waypoint-name  wp)  name  (waypoint-duration  wp)  duration  (support-state  wp)  support 
(rotation-reference-point  wp)  reference  (sole-angle  wp)  sole  (heel-position  wp)  heel 
(heel-position  wp)  heel  (toe-position  wp)  toe  (ankle -position  wp)  ankle))) 

(defmethod  update-waypoint-list  ((foot  biped-foot)  stride-increment) 

(let*  ((list  (waypoint-list  foot))  (waypoint  (first  list)) 

(new-waypoint  (update -waypoint  waypoint  stride-increment)) 

(new-list  (append  (rest  list)  (list  new-waypoint)))) 

(setf  (waypoint-list  foot)  new-list))) 

(defmethod  update-waypoint  ((waypoint  biped-foot)  stride-increment) 

(setf  (heel-position  waypoint)  (vector-add  stride-increment  (heel-position  waypoint)) 
(toe-position  waypoint)  (vector-add  stride-increment  (toe-position  waypoint)) 
(ankle-position  waypoint)  (vector-add  stride-increment  (ankle-position  waypoint))) 
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waypoint) 

(defmethod  update-node-positions  ((foot  biped-foot)) 

(let*  ((support-state  (support-state  foot))) 

(case  support-state 

((heel  sole)  (update-node-positions-from-heel  foot)) 

(toe  (update-node-positions-from-toe  foot)) 

(swing  (update-node-positions-from-ankle  foot))))) 

(defmethod  update-node -positions-from-heel  ((foot  biped-foot))  ;Position  is  x-y  list  in  x-y  plotter 
(let*  ((sole-angle  (sole-angle  foot))  (si  (sole-length  foot))  coordinates. 

(heel-to-ankle-angle  (+  sole-angle  (sole-to-ankle-angle  foot))) 

(heel-position  (heel-position  foot))  (htal  (heel-to-ankle-length  foot)) 

(ankle-x-position  (+  (first  heel-position)  (*  (cos  heel-to-ankle-angle)  htal))) 
(ankle-y-position  (+  (second  heel-position)  (*  (sin  heel-to-ankle-angle)  htal))) 
(toe-x-position  (+  (first  heel-position)  (*  (cos  sole-angle)  si))) 

(toe-y-position  (+  (second  heel-position)  (*  (sin  sole-angle)  si)))) 

(setf  (ankle-position  foot)  (list  ankle-x-position  ankle-y-position) 

(toe-position  foot)  (list  toe-x-position  toe-y-position)))) 

(defmethod  update-node-positions-from-toe  ((foot  biped-foot));Position  is  in  x-y  plotter  coordi¬ 
nates. 

(let*  ((sole-angle  (sole-angle  foot))  (si  (sole-length  foot)) 

(heel-to-ankle-angle  (+  sole-angle  (sole-to-ankle-angle  foot))) 

(toe-position  (toe-position  foot))  (htal  (heel-to-ankle-length  foot)) 

(heel-x-position  (-  (first  toe-position)  (*  (cos  sole-angle)  si))) 

(heel-y-position  (-  (second  toe-position)  (*  (sin  sole-angle)  si))) 

(ankle-x-position  (+  heel-x-position  (*  (cos  heel-to-ankle-angle)  htal))) 

(ankle-y-position  (+  heel-y-position  (*  (sin  heel-to-ankle-angle)  htal)))) 

(setf  (ankle-position  foot)  (list  ankle-x-position  ankle-y-position) 

(heel-position  foot)  (list  heel-x-position  heel-y-position)))) 

(defmethod  update-node -positions-from-ankle  ((foot  biped-foot))  ;Position  is  in  x-y  plotter 
(let*  ((sole-angle  (sole-angle  foot))  (si  (sole-length  foot))  coordinates. 

(heel-to-ankle-angle  (+  sole-angle  (sole-to-ankle-angle  foot))) 

(ankle-position  (ankle-position  foot))  (htal  (heel-to-ankle-length  foot)) 

(heel-x-position  (-  (first  ankle-position)  (*  (cos  heel-to-ankle-angle)  htal))) 

(heel-y-position  (-  (second  ankle-position)  (*  (sin  heel-to-ankle-angle)  htal))) 

(toe-x-position  (+  heel-x-position  (*  (cos  sole-angle)  si))) 

(toe-y-position  (+  heel-y-position  (*  (sin  sole-angle)  si)))) 

(setf  (toe-position  foot)  (list  toe-x-position  toe-y-position) 

(heel-position  foot)  (list  heel-x-position  heel-y-position)))) 

(defmethod  draw-foot  ((foot  biped-foot)  (recorder  x-y-recorder)  line-width  scale)  ;  Does  not  draw 

toes. 

(let*  ((heel  (heel-position  foot))  (ankle  (ankle-position  foot))  (toe  (toe-position  foot))) 

(and  (draw-wide-line-in-window  recorder  line-width  scale  scale  ankle  heel) 
(draw-wide-line-in-window  recorder  line-width  scale  scale  heel  toe) 
(draw-wide-line-in-window  recorder  line-width  scale  scale  toe  ankle)))) 

(defun  start  () 

(setf  right-foot  (make-instance  'biped-foot)) 

(make-waypoint-list  right-foot  6) 

(initialize-waypoint  right-foot  0  'heel-strike  .1  'heel  'heel  .4  '(0  0)  nil  nil) 

(update-node-positions  (elt  (waypoint-list  right-foot)  0)) 

(initialize-waypoint  right-foot  1  'foot-flat  .5  'heel  'heel  .0  '(0  0)  nil  nil) 

(update-node-positions  (elt  (waypoint-list  right- foot)  1)) 

(initialize-waypoint  right-foot  2  'toe-off  .1  'toe  'toe  -.7  nil  '(24  0)  nil) 

(update-node-positions  (elt  (waypoint-list  right-foot)  2)) 

(initialize-waypoint  right-foot  3  'early-swing  .1  'swing  'ankle  -.1  nil  nil  '(40  15)) 
(update-node-positions  (elt  (waypoint-list  right-foot)  3)) 
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(initialize -waypoint  right-foot  4  'mid-swing  .1  'swing  'ankle  0  nil  nil  '(80  15)) 
(update-node-positions  (elt  (waypoint-list  right-foot)  4)) 

(initialize-waypoint  right-foot  5  'late-swing  .1  'swing  'ankle  .4  nil  nil  '(120  15)) 
(update-node-positions  (elt  (waypoint-list  right-foot)  5)) 

(setf  recorder- 1  (make-instance  'x-y-recorder)) 

(initialize-recorder  recorder- 1  900  600  '(90  500)) 

(draw-coordinate-axes  recorder- 1 ))) 

(defun  tfl  (index)  (draw- foot  (elt  (waypoint-list  right- foot)  index)  recorder- 1  3  2)) 
(defun  tf2  ()  (draw-foot  (elt  (waypoint-list  right-foot)  0)  recorder-1  3  2) 
(update-waypoint-list  right-foot  '(120  0))) 
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APPENDIX  E 


This  Appendix  presents  the  Matlab  simulation  program  that  processes  the  real 
data.  This  program  was  discussed  in  Chapter  VI.  (Comments  are  highlighted  in  green.) 

%  The  first  part  of  the  code  is  the  "Factored  Quaternion  Algorithm", 

%  written  by  Conrado  Aparicio,  and  receives  raw  data  from  the  sensors  and  transforms  it  first  to 
quaternions 

%  and  then  to  angles.  The  second  part  of  the  code  is  written  by  Pantazis 
%  Ioannis  and  implements  the  data  to  a  simulation  which  represents  the 
%  motion  of  the  lower  extremities. 

%  PART  I  -  written  by  Conrado  Aparicio  -  September  2004 

format  long 
close  all, 
clear  alfclc 
q_opt-|  |; 

R_cal=[]; 

%  Kalman  Filter  initialization 
%  %  ===================== 

qhat=[]; 
deltat  =  0.01; 
taol=0.5; 
tao2=0.5; 
tao3=0.5; 

Dl=50; 

D2=50; 

D3=50; 

sigmal=sqrt(.005); 

sigma2=sqrt(.005); 

sigma3=sqrt(.005); 

sigma4=sqrt(.000 1  );%sqrt(  1 .42e-4); 

sigma5=sqrt(.0001);%sqrt(1.42e-4);%le-6;%sqrt(5.69e-5); 

sigma6=sqrt(.0001);%sqrt(1.42e-4);%sqrt(4.03e-5); 

sigma7=sqrt(.0001);%sqrt(1.42e-4);%le-6;%sqrt(1.18e-4); 

qll  =  (Dl/(2*taol))*(l-exp(-2*delta_t/taol)); 

q22  =  (D2/(2*tao2))*(l-exp(-2*delta_t/tao2)); 

q33  =  (D3/(2*tao3))*(l-exp(-2*delta_t/tao3)); 

Q  =  [ql  1  0  0  0  0  00; 

0  q22  0  0  0  0  0; 

0  0  q33  0  0  0  0; 

0  0  0  0  0  0  0; 

0  0  0  0  0  0  0; 

0  0  0  0  0  0  0; 

0  0  0  0  0  0  0]; 

R_k  =  [sigmalA2  0  0  0  0  0  0; 

0  sigma2A2  0  0  0  0  0; 

0  0  sigma3A2  0  0  0  0; 

0  0  0  sigma4A2  0  0  0; 

0  0  0  0  sigma5A2  0  0; 
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0  0  0  0  0  sigma6A2  0; 

0  0  0  0  0  0  sigma7A2]; 


phil  =  [exp(-delta_t/taol)  0  0; 

0  exp(-delta_t/tao2)  0; 

0  0  exp(-delta_t/tao3)]; 

phi_2  =  zeros(3,4); 

I  =  eye(7); 

H  =  I; 

P  =  randn(7,l); 

Pkminus  =  10000*I;%diag(P.*P); 
xhat_minus=[]; 

xhat_minus(:,l)=[0  0  0  0.5  0.5  0.5  0.5]'; 

%  Load  Sensor  calibrating  values 
Sensor04 

%Load  Sensor  Measurements 

load  rightthighfullstep.txt  %  randomArmMotion.txt%  NED05.txt% 
MargData05aboutZ_CIU3.txt%  superfast. txt%  NEDpitchUp.txt%  %rotation  about  x-axis  at  a  rate 

of  60  degrees/sec 

MargData  =  rightthighfullstep;  %  randomArmMotion;%  NED05;%  MargData05aboutZ_CIU3;% 
superfast;%  NEDpitchUp;% 

%Get  reference  Magnetic  measurement  for  NED  orientation  of  the  sensor 
global  Mref;  %  magnetic  ref 

Mref=[MagRef(l);MagRef(2)];  %Get  only  x  and  y  components  for  the  factored  quaternion  algo¬ 
rithm 

Mref=Mref/norm(Mref); 

[aa  bb]=size(MargData); 

MargData=MargData( :  ,2  :bb); 

M=MargData(  1  :aa,9 : 1 l)/4096*3; 

A=MargData(  1  :aa,6 : 8)/4096*3 ; 

R=MargData(l:aa,3:5)/4096*3; 

%  Data  Calibration 
[row  colum]=size(M); 

Meal  =  (M-repmat(magnull,row,l))-*repmat(magscale,row,l); 

Acal  =  (A-repmat(accelnull,row,  1 )).  *repmat(accelscale,row,  1 ); 
tau  =  10; 

alpha  =  l-(delta_t/tau); 

E1PG  =  (l+alpha)/(2*alpha); 

qhat_minus  =  [.5  .5  .5  .5]';  %  it  hs  to  be  a  unit  quaternion 

rr=[]; 

for  k=l:aa 

ratenull  =  (alpha*ratenull)  +  (l-alpha)*R(k,:);rr(k,:)  =  ratenull; 

R  fdt  =  E1PG  *  (R(k,:)  -  ratenull). *ratescale; 

R  eal  =  [  R  eal ;  R  fdt  ]; 

q_factt=fact_quat3([M_cal(k,:)'  A_cal(k, :)']);  %  returns  the  factored  quaternion  [qO  ql  q2  q3] 

%  q_factt=Quest([M_cal(k,:)'  A_cal(k, :)']); 
q_fact  =  q_factt; 
error=q_fact-qhat_minus; 
if  norm( error)  >1.9 
q_fact=-q_fact; 
end 

q_opt=[q_opt  q_factt]; 
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%  %  Kalman  filter 
omega  =  R_cal(k,:); 
z_k  =  [omega';  q_fact]; 

K_k  =  P_k_minus*inv(  Pkminus  +  R_k); 
xhat(:,k)  =  xhat_minus(:,k)  +  K_k*(z_k  -  (xhat_minus(:,k))); 
xhat(4:7,k)  =  xhat(4:7,k)/norm(xhat(4:7,k));  %  Normalize  the  quaternion  estimate 

P_k  =  (I  -  K_k)*P_k_minus*[(I  -  K_k)]'  +  K_k*R_k*[K_k]'; 
p(k)  =  trace(Pk); 
pm(k)  =  trace(Pkminus); 

xhat_minus(l,k+l)  =  xhat(l,k)  +  delta_t*(-l/taol)*xhat(l,k); 
xhat_minus(2,k+l)  =  xhat(2,k)  +  delta_t*(-l/tao2)*xhat(2,k); 
xhat_minus(3,k+l)  =  xhat(3,k)  +  delta_t*(-l/tao3)*xhat(3,k); 
xhat_minus(4,k+l)  =  xhat(4,k)  +  delta_t*(- 
0.5)*(xhat(l,k)*xhat(5,k)+xhat(2,k)*xhat(6,k)+xhat(3,k)*xhat(7,k)); 

xhat_minus(5,k+l)  =  xhat(5,k)  +  delta_t*(0.5)*(xhat(l,k)*xhat(4,k)- 
xhat(2,k)*xhat(7,k)+xhat(3,k)*xhat(6,k)); 

xhat_minus(6,k+l)  =  xhat(6,k)  +  delta_t*(0.5)*(xhat(l,k)*xhat(7,k)+xhat(2,k)*xhat(4,k)- 
xhat(3,k)*xhat(5,k)); 

xhat_minus(7,k+l)  =  xhat(7,k)  +  delta_t*(0.5)*(- 
xhat(l,k)*xhat(6,k)+xhat(2,k)*xhat(5,k)+xhat(3,k)*xhat(4,k)); 

xhat_minus(4:7,k+l)  =  xhat_minus(4:7,k+l)/norm(xhat_minus(4:7,k+l));  %Normalize 
the  quaternion  projection 

qhat_minus  =  xhat_minus(4:7,k+l); 

phi_k=[  phil,  phi_2 

J 

[-xhat(5,k)*0.5*delta_t  -xhat(6,k)*0.5*delta_t  -xhat(7,k)*0.5*delta_t  1 
xhat(l,k)*0.5*delta_t  -xhat(2,k)*0.5*delta_t  -xhat(3,k)*0.5*delta_t; 

xhat(4,k)*0.5*delta_t  -xhat(7,k)*0.5*delta_t  xhat(6,k)*0.5*delta_t  xhat(l,k)*0.5*delta_t  1 
xhat(3,k)*0.5*delta_t  -xhat(2,k)*0.5*delta_t; 

xhat(7,k)*0.5*delta_t  xhat(4,k)*0.5*delta_t  -xhat(5,k)*0.5*delta_t  xhat(2,k)*0.5*delta_t  - 
xhat(3,k)*0.5*delta_t  1  xhat(l,k)*0.5*delta_t; 

-xhat(6,k)*0.5*delta_t  xhat(5,k)*0.5*delta_t  xhat(4,k)*0.5*delta_t  xhat(3,k)*0.5*delta_t 
xhat(2,k)*0.5*delta_t  -xhat(l,k)*0.5*delta_t  1  ]]; 

Pkminus  =  phi_k*P_k*phi_k'  +  Q;P_k_minus=diag(diag(P_k_minus)); 
qhatt  =  [xhat(4:7,k)];  qhatt=qhatt/norm(qhatt);  %  quaternion  estimate 
qhat  =  [qhat  qhatt]  ;%_minus]; 
end 

time=l:row; 

q_opt_time=[time'  q_opt'];  %each  column  has  4  elements,  the  quaternion,  each  row  has  as  many 
columns  as  the  number  of  samples  we  get  from  each  sensor 
qhat  time  =[time'  qhat']; 

sim('RocketManual2')  %  computes  the  euler  Angles  from  the  factored  quaternion  and  the  eider 
angles  from  the  quaternion  estimate  (qhat) 

%  PART  II  -  written  by  Pantazis  Ioannis  -  May  2005 

EA_thigh=EulerAngles2(l:200,:)*  180/pi; 

EA2_thigh=EA_thigh*pi/l  80; 

[EA_thigh( : ,  1  ),EA_thigh(:  ,2)] ; 

11=0.45;  %  length  of  thigh 

12=.4;  %  length  of  shank 

13=11;  %  length  of  supporting  thigh 

14=12;  %  length  of  supporting  shank 
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%  insert  the  measurements  concerning  the  shank 

EA_shankl=[ones(105,l);(-l)*ones(18,l);ones(77,l)]; 

EA_shank2 1=[76.294445 167691 82;76.25898709034478;76. 1 94875463 15098;76. 168765986 1580 
9;76. 1 470 1087758472;76. 13205 1 8469556 1;76.08909707746278;76. 066 1529784328 1;76. 03 10772 160291 8 
;76.00267088831822;75.978051 1761 7808;75. 9521 3 134933447; 

75.91059679089688;75.87726622439567;75.86125095438634;75.83  87943799 10 14;75.82728928 
190740;75.8 1 576825557694;75. 775033 18548564;75. 7 1 799423030409;75. 7003  8450334236;75.679329402 
15937;75. 65497485953675;75. 6390707 1723236;75. 6 1820740004332; 

75.6040294381 1820;75.60977076906335;75.61980139659842;75.62769201770324;75. 60215360 
164034;75. 6 12722284091 1 1;75. 65705450258581  ;75.73709960389024;75.84696375579564;75. 997061326 
52270;76.16880028674828;76.36437252029360;76. 5672 1508909990; 

76.76406981507859;76.93718130314373;77. 092131230925 13;77. 27021 122229672;77. 46998059 
975337;77.7246 1968 11 53 19;78.01255051367656;78.3461 9835 170764;78.650921 6 1335826;78. 9 10068428 
37263;79.08673939683688;79.12945609917937;79. 00986724369764; 

78.746547807 18427;78.30149067710207;77. 6918 1885649395;76.90853376704095;76. 07384040 
857325;75.17066790220079;74. 19519181917286;73. 15244801705703;71.87516879878763;70. 344366475 
1 6506;68. 507 1 176345663  8;66.55390679423755;64. 7 1960684976 156; 

63. 21278 1 7890 1 569;62. 38973 1 32589867;6 1.951 049853 15764;6 1.570 1861 8744907;6 1.45348320 
996450;61. 60 108340530854;61.95084572445634;62. 45850297697209;63. 07 100844330289;63. 7425 13450 
87574;64. 51 482663057500;65. 35736888059573  ;66. 28468290898759; 

67. 25003524563567;68. 26853240 169430;69. 342641 4052692 1;70. 504 1082 1702026;7 1.656 19565 
232599;72.896 15799729 13 1;74. 153 13543  801 167;75.506699 11 666596;76.83 196460070 100;78. 126576679 
21566;79.42018012292473;80.66888844115651;81. 80751424040940; 

82.87 1647 19398398;83.81 1 632504 14097;84. 75524265 117964;85. 589783  89448480;86. 36834087 
159643;87. 0408293 161 1669;87.59387353991258;88.03799362358836;88. 3788598978 1506;88. 593443251 
1 6685;88. 68307925047637;88. 7 1080438393749;88. 71 140944157961; 

88.68902571 636906;88.679433 1470028 1;88.69079008593859;88.71 144487 135848;88.74052266 
313946;88.72628711327272;88.71500158853635;88.72796933630312;88.75942505068528;88. 842209650 
10768;88.95281543322396;89.07326517809332;89. 17204 159030709; 

89. 1975672787220 1;89. 2359679985345 1;89.35070400569320;89.58058055957454;89. 78898070 
994397;90. 00000000000000;89. 66468221 480379;89. 3 1286494253048;89. 1 5089648 109229;88. 979979262 
02881;88.00004656325169;86.82758351286529;85. 89528548240612; 

84.67784626560631;83.42881 197808 107;82. 404395 16283 183  ;8 1.504 17450888546;80. 57792003 
9343  85;79.6350757 1 898 1 84;78. 75645077084 195;77. 98360 1 82643883  ;77.31 39722465 1268;76.67 11 52830 
43033  ;76. 0944958689 1236;75.64312935767900;75. 25382425404361]; 

EA_shank22=(10*ones(58,l))+[74.96762268464075;74.74227295509245;74.540467194331 17;7 
4.39666956739625;74.30680597040568;74.21179897718174;74.1471 1 78926 1244;74. 093494721 5927 1;74 
. 0576771 92229 16;74. 05269364692 1 63  ;74. 083 1 9409250444;74. 19 124679040 1 14;74. 3254 1353374622; 

74. 443450 14544456;74. 55925551 374570;74. 6631 928528845 1;74. 76909825703933  ;74. 874 15572 
82924 1;74. 9639555227459 1;75. 02232006 152686;75. 08849695280063  ;75. 14764941 183  837;75. 197328345 
40049;75. 30 1840680 1 6054;75. 3  7452777998756;75. 43375807847781; 

75. 49222669446675;75. 552965 1 50559 12;75. 659623 1 4352377;75. 764681 040797 19;75. 85264888 
3 13724;75.94465649824930;76.027 1846091 364 1;76.08757573468162;76. 1571 73090508 14;76. 23473 1640 
59698;76.28381277471297;76.30466000158124;76.3485 1845 15921 1; 

76. 405595 1 8259376;76. 50 12273  8792454;76. 6081 764759 1622;76. 699751 584723  87;76. 79 174503 
757280;76.88 188601 88342 1;76.94050214480657;77.00004525752304;77.07020594886336;77. 128339833 
0 1229;77. 1 9800263  84827 1;77. 26080 170 146 199;77. 32308854608472; 

77. 373093  73056630;77. 41 8092478232 14;77. 4485 180762838;77. 4906435586763 1;77. 523274853 
59959;77. 56 15975 1733415]; 

EA_shank2=[EA_shank2 1  ;EA_shank22] 

EA_shank=[EA_shankl,EA_shank2] 

EA2_shank=EA_shank*pi/180; 


%  insert  the  measurements  concerning  the  supporting  thigh 
EAsupthigh  1 =[(- 1  )*ones(92, 1  );ones(  1 08, 1)] ; 
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EA_sup_thigh2=l  00*[0.8 1208896 1 804 12;0.8 12623397 12807;0.8 12654933 1 5408;0. 8 12209 13500 
677;0.8 11 44036 1556 15;0.81 07 1808573534;0.80981 9620 17485;0. 8088035939359 1;0.80746761 4463  77;0. 8 
0594448014483;0.80442702880745;0.80270465682224;0. 801 1 1690906816; 

0. 799840769 1 6483  ;0. 79882 14628858 1;0. 798 150 1 45 18627;0. 79802944365901  ;0. 798204789451 39;0. 7985 
1 162254753;0.79893 172330094;0. 7996 1674445 157;0. 8005 197648 1299;0. 80 150234479 199;0. 8024656448 
8709;0.80340540450625;0.80425508353976;0. 80580 189842780; 

0.8075347463  7967;0.8093 1297206253  ;0.8 12 11046676479;0. 8 1555466830462;0.81940528492108;0. 8237 
669 1 757932;0. 828692667343  73  ;0. 83375530376260;0. 838990388303 18;0. 8441 078963  8780;0. 848726 1986 
2057;0.85277684755907;0. 856221 62730903  ;0. 85871 539566073; 

0. 8603  8767525222;0. 86 185202656324;0. 863 16863370845;0. 8645 10609636 10;0.86575420898734;0. 8673 
7408650786;0. 8688828851 9753  ;0. 87070 124445594;0. 872798709345 12;0. 8750454 1097800;0. 8770252367 
5087;0. 878755 127379 1 8;0. 880 1 90440 10944;0. 88 104368 162488; 

0.88205093560789;0. 882486838281 16;0.88277389877805;0.88242912320257;0. 8818570 1610579;0.8810 
3720481551;0.88032677328632;0.87951421975067;0.87867554226063;0.87782768514641;0. 8769670672 
1 120;0.87587949036489;0.87509644584962;0. 87385458 102020; 

0. 87302842995390;0. 872 1 3672570005;0. 87077 1740 12203  ;0.86979077655528;0. 868599 1 6583 152;0. 8681 
0459136526;0.86825226573316;0.86843746482671;0.86855047479725;0.86906305896723;0. 8692 15353  8 
2 112;0. 869 18071 520 107;0.86884665552536;0.86900 12083 1288; 

0.86925 168007305;0.86929640568779;0.869641 822015 18;0.87044302084752;0.87 14172022039 1;0.8726 
74326 14 132;0. 8743543  84761 43  ;0.87623540651 94 1  ;0.87869975980009;0.88 1 18829595354;0.8839498458 
3  836;0. 88728 1491 77098;0. 8905578972534 1;0. 892705 19709689; 

0.89444222469540;0.89636057649710;0.89851608472366;0.90000000000000;0.89939 12 1446693  ;0. 8989 
260 1 342380;0. 898009 19721 537;0.89679477445422;0. 89708397 1 7787 1;0. 89642727736 140;0. 8943684459 
3  802;0. 89 198094539749;0.8893  872777571 3  ;0.886445  89843 121; 

0. 883 1 9750036 185;0. 879473907 17692;0. 87626 1 49993330;0. 8739075 1778260;0. 8720667249 1652;0. 8703 
1 10307598 1;0. 86876227978 168;0.86708503150964;0.86506574286288;0.862960367 12 18 1;0.86 10720759 
6335;0.85930153615555;0.85777447793126;0. 856 15656328293; 

0.85444275743586;0.8531 576 1487225;0.852 156062 16799;0. 85 11 34934 15425;0.8502541 8 108227;0. 8495 
848288 1612;0. 8489 1694999846;0.8484631 9848 197;0.84796746717649;0.84736 191 53680 1;0.846639 1061 
4098;0.8462082265 1993;0. 84581560213 136;0. 84564420135650; 

0.8458 10 10 143275;0.84605281098612;0.84631737536776;0. 8468065965 1290;0.84742620126069;0. 8479 
5808905599;0. 84881 535 117469;0.84957736632996;0.85052839588649;0.85139696244070;0.85249 14 194 
0626;0. 85333 100890564;0.85409963602872;0. 854770958 19955; 

0.85528398701 6 16;0.85600758560993;0.8569861 583 1678;0. 85791 360989 160;0.859 142984843 13;0. 8605 
2453397959;0.86212761551655;0.86347395888432;0.86501772736091;0.86662865223590;0. 868 1480864 
2568;0.86963028359508;0.87099725343070;0. 87228946640297; 

0. 87350983481 403  ;0. 87441944878053  ;0. 87505588410618;0. 87545 19481 18 10;0. 8756659 1 37951 3;0. 8757 
828 1060499;0. 87604 15236541 3  ;0. 87607662 1 67363  ;0. 876 19052 1074 1 1;0. 87603 1879208 16;0. 8760 168499 
1 59 1;0.87587268088960;0.87570092494379;0. 8756 1897 143  800; 

0. 8757953  84950 10;0. 8759655553 1847;0. 8762370 1 1 30239;0. 8764854 16059 13  ;0. 876799923  84545  ;0. 8767 
472256501 1;0. 87673 1 12666920;0. 87668087740364;0. 87662732762323  ;0. 876437657868 15;0. 8765705875 
7300;0. 876743  85412704;0. 87682692 108 184;0. 8770 1356480309; 

0.8772801 6683646;0. 87759209059653  ;0.87782792992677;0. 87809 139583 122;0. 878423779873  83]; 
EA_sup_thigh=[EA_sup_thigh  1  ,EA_sup_thigh2]; 

EA2_sup_thigh=E  A_sup_thigh*pi/ 180; 


%  insert  the  measurements  concerning  the  supporting  shank 
EA_sup_shank  1  =ones(200, 1 ) ; 

EA_sup_shank2=100*[0.88145694377428;0.880986006661 14;0. 880540494483 16;0. 8802302897 
2 148;0. 87988768544396;0. 879405004 14682;0. 878876 1 1740096;0. 8786592 1072628;0. 878572050263 14;0. 
8782 1 947333709;0. 87782657624405;0. 877469936063  74;0. 8770861 873  8439; 

0. 87704 186548039;0. 87691270262198;0. 87666895 151 648;0.87653777948691;0. 876055 15361242;0.8757 
1 896762354;0.87546495794080;0. 875 18382769547;0. 874835572 14028;0. 87453 106662 142;0. 87435840 10 
5 168;0. 874 1431 8038022;0. 87382095 179957;0. 87344674747826; 

0.873041 09782893  ;0.87261302440952;0.871 910252069 10;0. 87 1371 5483 1996;0.87075279774303;0. 8702 
4369685 157;0. 86998 1 34997923  ;0. 86962798904008;0. 86901 692863969;0. 86858837 1 395 11;0. 868 15093 17 
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3475;0.867647882963 17;0. 867 15009776390;0. 86640548508302; 

0. 86560340739622;0. 8650943 1728253  ;0. 86468 125942468;0. 864 14023 146833  ;0.86369927678767;0. 8632 
65253 14249;0.86273906080225;0. 862 143630927 18;0. 861 3  89760 18066;0.86055635685959;0. 8597764252 
9815;0.85919995116320;0.85867984855068;0. 85806692604993; 

0.85759651 13 1424;0.85729425579491;0.85704651849988;0.8569948459960;0.856957 16283 17 1;0.85674 
838608400;0. 856661 72665302;0.85728544277858;0. 85805935399425;0. 85907543  901 582;0. 860 17202032 
335;0.86105844 168805;0.86 13805 1081 723;0.861082 1653726 1 ; 

0. 8609 1 3986 129 15;0. 86 130 172323762;0. 86 12778068998 1;0. 86 105054032341  ;0. 860 14674306744;0. 8582 
8637140000;0.85696933153254;0.85708190712982;0.85749302177820;0.85639037733685;0. 8543430626 
1 477;0. 8521 3423206423  ;0. 85040661 566885;0. 8484249 1322758; 

0. 8456 1 6264403  86;0. 84245 187363290;0. 83880882589258;0. 8356895 1788896;0.83334277804874;0. 83 15 
3074306578;0. 828776702441 91  ;0.82572426475620;0.82270846905290;0. 8 196 1457664385;0. 81655 198 18 
4184;0.81374336616149;0.81 138969873 123  ;0.8094803 1661 181; 

0.80709 141 643393  ;0.80358808779429;0. 799692 18207858;0.79558293706615;0. 79056558748 186;0.7849 
9695293050;0.77887065592652;0. 77273043779037;0. 766803547 10089;0. 7607962883  8865;0. 754832 1341 
9288;0.74924786554812;0.74409435379501;0. 73908834546689; 

0.733478 12236586;0.72742784258 14 1  ;0. 720 1587 1709967;0.71323493265998;0.70661 00290 1 194;0.7005 
00933672 19;0.69550838040394;0. 692 14835 103944;0. 690848998 1 1589;0. 690934858 13566;0. 69 16807734 
7566;0.69265661 64220 1;0.69247553867062;0.692 16 12 1849557; 

0. 69 163244766057;0. 6897070525738 1;0. 68532635044989;0. 678043  82550346;0.67229039660690;0. 6691 
0070249944;0. 66649790 163919;0. 6639 147002 1779;0.65949068520586;0. 652786171 18391  ;0. 6433326266 
8914;0.63 154186100421;0.62103367898254;0. 60658423549736; 

0. 5934862568405 1;0. 580486790200 1 3  ;0. 56806001 505859;0.55659095555474;0. 546050092491 10;0.5360 
6403925300;0.52689402564668;0. 5 182 121 9872584;0. 5 1040399677364;0. 503347649494 19;0. 4972463928 
3650;0. 492062 1087255 1;0. 48789 109039820;0. 485 11 3302973  87; 

0. 48379757207430;0. 48341 57045677 1;0. 483  87024 1 13309;0. 48489905 137038;0. 4863 14570 15798;0. 4879 
097 1207843  ;0.48925540665776;0. 49084942576778;0. 49239520572320;0. 494 1 0520935 197;0. 4954636342 
6479;0. 49655825033453  ;0.49728764890669;0. 49760375286584; 

0.4974 13 13338138;0. 4973 129279972 1;0. 49724386 186680;0.49755672530020;0.49798005476640;0. 4984 
8808906330;0.49958173907326;0.50097921038093;0.50237081075510;0.50357684339041;0. 5047764495 
7548;0.50579643078758;0.50689418315434;0. 508094 17077050; 

0.509 14482003942;0.5 1033527 1 95 124;0.5 1 1499029421 17;0. 5 127842673  8438;0.5 142490 1296548;0. 5 154 
8049524739;0.5167438631 1984;0.5 179744 1727474;0.5 187975250 1351  ;0.5 19269293 15 194;0. 5 196774093 
4591  ;0. 52006 181 1281 13;0. 52065026491371  ;0. 52120862944548; 

0. 521 58568082287;0. 52 190638988448;0.52209055246057;0.52227347565176;0. 522605 19079751]; 
EA_sup_shank=[EA_sup_shank  1  ,EA_sup_shank2] ; 

EA2_sup_shank=EA_sup_shank*pi/180; 


%  corec=(90-max(EA_thigh(:,2)))*pi/180; 

x0=l; 

y0=0; 

x=[x0,x0-14*cos(pi-EA2_sup_shank(l,2)),x0-14*cos(pi-EA2_sup_shank(l,2))- 
13  *cos(EA2_sup_thigh(  1 ,2)),xO-14*cos(pi-EA2_sup_shank(  1 ,2))-13  *cos(EA2_sup_thigh(  1,2))- 
1 1  *cos(EA2_thigh(  1 ,2)),xO-14*cos(pi-EA2_sup_shank(  1 ,2))-13  *cos(EA2_sup_thigh(  1,2))- 
1 1  *cos(EA2_thigh(  1 ,2))-12  *cos(EA2_shank(  1,2))] 

y=[yO,yO+14*sin(pi-EA2_sup_shank(l,2)),yO+14*sin(pi- 

EA2_sup_shank(l,2))+13*sin(EA2_sup_thigh(l,2)),y0+14*sm(pi- 

EA2_sup_shank(  1 ,2))+13  *sin(EA2_sup_thigh(  1 ,2))-l  1  *sin(EA2_thigh(  1 ,2)),y0+14*sin(pi- 
EA2_sup_shank(l,2))+13*sin(EA2_sup_thigh(l,2))-ll*sin(EA2_thigh(l,2))-12*sin(EA2_shank(l,2))] 
nframes=length(EA2_thigh); 
h=plot(x,y) 
set(h,'MarkerSize',5); 
axis([0  2  0  1]) 
fork=l:200 
k 

omega=pi-EA2_sup_shank(k,2); 
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if  EA_sup_thigh(k,  1  )>=0 
chi=  1 80-EA_sup_thigh(k,2); 
else 

chi=EA_sup_thigh(k,2 ) ; 
end 

if  EA_thigh(k,  1  )>=0 
psi=EA_thigh(k,2); 
else 

psi=l  80-EA_thigh(k,2); 
end 

if  EA_shank(k,  1  )>=0 
phi=E  A_shank(k,2 ) ; 
else 

phi=l  80-EA_shank(k,2); 
end 

x=[x0,x0-14*cos(pi-EA2_sup_shank(k,2)),x0-14*cos(pi-EA2_sup_shank(k,2))- 

13*cos(chi*pi/180),x0-14*cos(pi-EA2_sup_shank(k,2))-13*cos(EA2_sup_thigh(k,2))- 

ll*cos(psi*pi/180),x0-14*cos(pi-EA2_sup_shank(k,2))-13*cos(EA2_sup_thigh(k,2))-ll*cos(psi*pi/180)- 

12*cos(phi*pi/180)]; 

y=[yO,yO+14*sin(pi-EA2_sup_shank(k,2)),yO+14*sin(pi- 

EA2_sup_shank(k,2))+13*sin(chi*pi/180),y0+14*sin(pi- 

EA2_sup_shank(k,2))+13*sin(EA2_sup_thigh(k,2))-ll*sin(psi*pi/180),y0+14*sin(pi- 

EA2_sup_shank(k,2))+13*sin(EA2_sup_thigh(k,2))-ll*sin(psi*pi/180)-12*sin(phi*pi/180)]; 

set(h,'XData',x,'YData',y) 

M  =  getframe; 
x(l,5) 
end 
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